Initial import of WAVE_ROVER_V1.0

.gitignore

@@ -0,0 +1,7 @@
+.DS_Store
+build/
+*.elf
+*.map
+*.bin
+*.hex
+.arduino/

AK09918.cpp

@@ -0,0 +1,285 @@
+/*
+    AK09918.cpp
+    A library for Grove - IMU 9DOF(ICM20600 + AK09918)
+
+    Copyright (c) 2018 seeed technology inc.
+    Website    : www.seeed.cc
+    Author     : Jerry Yip
+    Create Time: 2018-06
+    Version    : 0.1
+    Change Log :
+
+    The MIT License (MIT)
+
+    Permission is hereby granted, free of charge, to any person obtaining a copy
+    of this software and associated documentation files (the "Software"), to deal
+    in the Software without restriction, including without limitation the rights
+    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+    copies of the Software, and to permit persons to whom the Software is
+    furnished to do so, subject to the following conditions:
+
+    The above copyright notice and this permission notice shall be included in
+    all copies or substantial portions of the Software.
+
+    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+    THE SOFTWARE.
+*/
+
+
+#include "AK09918.h"
+
+AK09918::AK09918() {
+    _addr = AK09918_I2C_ADDR;
+}
+
+
+AK09918_err_type_t AK09918::initialize(AK09918_mode_type_t mode) {
+    if (mode == AK09918_SELF_TEST) {
+        mode = AK09918_POWER_DOWN;
+    }
+    _mode = mode;
+
+    if (mode == AK09918_NORMAL) {
+        return AK09918_ERR_OK;
+    } else {
+        return AK09918::switchMode(_mode);
+    }
+}
+
+AK09918_err_type_t AK09918::isDataReady() {
+    if (!AK09918::readByte(_addr, AK09918_ST1, _buffer)) {
+        return AK09918_ERR_READ_FAILED;
+    } else {
+        if (_buffer[0] & AK09918_DRDY_BIT) {
+            return AK09918_ERR_OK;
+        } else {
+            return AK09918_ERR_NOT_RDY;
+        }
+    }
+}
+
+AK09918_err_type_t AK09918::isDataSkip() {
+    if (!AK09918::readByte(_addr, AK09918_ST1, _buffer)) {
+        return AK09918_ERR_READ_FAILED;
+    } else {
+        if (_buffer[0] & AK09918_DOR_BIT) {
+            return AK09918_ERR_DOR;
+        } else {
+            return AK09918_ERR_OK;
+        }
+    }
+}
+
+AK09918_err_type_t AK09918::getData(int16_t* axis_x, int16_t* axis_y, int16_t* axis_z) {
+    AK09918_err_type_t err = AK09918::getRawData(axis_x, axis_y, axis_z);
+    (*axis_x) = (*axis_x) * 15 / 100;
+    (*axis_y) = (*axis_y) * 15 / 100;
+    (*axis_z) = (*axis_z) * 15 / 100;
+
+    return err;
+}
+
+AK09918_err_type_t AK09918::getRawData(int16_t* axis_x, int16_t* axis_y, int16_t* axis_z) {
+    if (_mode == AK09918_NORMAL) {
+        AK09918::switchMode(AK09918_NORMAL);
+        bool is_end = false;
+        int count = 0;
+        while (!is_end) {
+            if (AK09918::_getRawMode() == 0x00) {
+                is_end = true;
+            }
+            if (count >= 15) {
+                return AK09918_ERR_TIMEOUT;
+            }
+            count ++;
+            delay(1);
+        }
+    }
+
+
+    if (!AK09918::readBytes(_addr, AK09918_HXL, 8, _buffer)) {
+        return AK09918_ERR_READ_FAILED;
+    } else {
+        *axis_x = (_buffer[1] << 8 | _buffer[0]);
+        *axis_y = (_buffer[3] << 8 | _buffer[2]);
+        *axis_z = (_buffer[5] << 8 | _buffer[4]);
+        if (_buffer[7] & AK09918_HOFL_BIT) {
+            return AK09918_ERR_OVERFLOW;
+        }
+        return AK09918_ERR_OK;
+    }
+}
+
+AK09918_mode_type_t AK09918::getMode() {
+    return _mode;
+}
+
+AK09918_err_type_t AK09918::switchMode(AK09918_mode_type_t mode) {
+    if (mode == AK09918_SELF_TEST) {
+        return AK09918_ERR_WRITE_FAILED;
+    }
+    _mode = mode;
+    if (!AK09918::writeByte(_addr, AK09918_CNTL2, mode)) {
+        return AK09918_ERR_WRITE_FAILED;
+    }
+    return AK09918_ERR_OK;
+}
+
+// 1.Set Power-down mode. (MODE[4:0] bits = “00000”)
+// 2.Set Self-test mode. (MODE[4:0] bits = “10000”)
+// 3.Check Data Ready or not by polling DRDY bit of ST1 register.
+// 4.When Data Ready, proceed to the next step. Read measurement data. (HXL to HZH)
+AK09918_err_type_t AK09918::selfTest() {
+    int32_t axis_x, axis_y, axis_z;
+    bool is_end = false;
+    AK09918_err_type_t err;
+    if (!AK09918::writeByte(_addr, AK09918_CNTL2, AK09918_POWER_DOWN)) {
+        return AK09918_ERR_WRITE_FAILED;
+    }
+    delay(1);
+    if (!AK09918::writeByte(_addr, AK09918_CNTL2, AK09918_SELF_TEST)) {
+        return AK09918_ERR_WRITE_FAILED;
+    }
+
+    while (!is_end) {
+        err = AK09918::isDataReady();
+        if (err == AK09918_ERR_OK) {
+            is_end = true;
+        } else if (err == AK09918_ERR_READ_FAILED) {
+            return AK09918_ERR_READ_FAILED;
+        }
+        delay(1);
+    }
+
+    // read data and check
+    if (!AK09918::readBytes(_addr, AK09918_HXL, 8, _buffer)) {
+        return AK09918_ERR_READ_FAILED;
+    } else {
+        axis_x = (int32_t)((((int16_t)_buffer[1]) << 8) | _buffer[0]);
+        axis_y = (int32_t)((((int16_t)_buffer[3]) << 8) | _buffer[2]);
+        axis_z = (int32_t)((((int16_t)_buffer[5]) << 8) | _buffer[4]);
+
+        if ((axis_x >= -200) && (axis_x <= 200) && (axis_y >= -200) && (axis_y <= 200) && \
+                (axis_z >= -1000) && (axis_z <= -150)) {
+            return AK09918_ERR_OK;
+        } else {
+            return AK09918_ERR_SELFTEST_FAILED;
+        }
+
+    }
+}
+
+AK09918_err_type_t AK09918::reset() {
+    if (!AK09918::writeByte(_addr, AK09918_CNTL3, AK09918_SRST_BIT)) {
+        return AK09918_ERR_WRITE_FAILED;
+    }
+    return AK09918_ERR_OK;
+}
+
+String AK09918::strError(AK09918_err_type_t err) {
+    String result;
+    switch (err) {
+        case AK09918_ERR_OK:
+            result = "AK09918_ERR_OK: OK";
+            break;
+
+        case AK09918_ERR_DOR:
+            result = "AK09918_ERR_DOR: Data skipped";
+            break;
+
+        case AK09918_ERR_NOT_RDY:
+            result = "AK09918_ERR_NOT_RDY: Not ready";
+            break;
+
+        case AK09918_ERR_TIMEOUT:
+            result = "AK09918_ERR_TIMEOUT: Timeout";
+            break;
+
+        case AK09918_ERR_SELFTEST_FAILED:
+            result = "AK09918_ERR_SELFTEST_FAILED: Self test failed";
+            break;
+
+        case AK09918_ERR_OVERFLOW:
+            result = "AK09918_ERR_OVERFLOW: Sensor overflow";
+            break;
+
+        case AK09918_ERR_WRITE_FAILED:
+            result = "AK09918_ERR_WRITE_FAILED: Fail to write";
+            break;
+
+        case AK09918_ERR_READ_FAILED:
+            result = "AK09918_ERR_READ_FAILED: Fail to read";
+            break;
+
+        default:
+            result = "Unknown Error";
+            break;
+    }
+    return result;
+}
+
+uint16_t AK09918::getDeviceID() {
+    AK09918::readBytes(_addr, AK09918_WIA1, 2, _buffer);
+    return (((uint16_t)_buffer[0]) << 8) | _buffer[1];
+}
+
+uint8_t AK09918::_getRawMode() {
+    if (!AK09918::readByte(0x0c, AK09918_CNTL2, _buffer)) {
+        return 0xFF;
+    } else {
+        return _buffer[0];
+    }
+}
+
+bool AK09918::readBytes(uint8_t addr,uint8_t reg,uint8_t num,uint8_t *buf)
+{
+  Wire.beginTransmission(addr);
+  Wire.write(reg);
+  if (Wire.endTransmission(false) != 0) {
+    return false;
+  }
+
+  if (Wire.requestFrom((int)addr, (int)num) != num) {
+    return false;
+  }
+  for (int i = 0; i < num; i++)
+  {
+    if (!Wire.available()) {
+      return false;
+    }
+    buf[i] = (uint8_t)Wire.read();
+  }
+  return true;
+}
+
+bool AK09918::readByte(uint8_t addr,uint8_t reg ,uint8_t *buf)
+{
+  Wire.beginTransmission(addr);
+  Wire.write(reg);
+  if (Wire.endTransmission(false) != 0) {
+    return false;
+  }
+
+  if (Wire.requestFrom((int)addr, 1) != 1) {
+    return false;
+  }
+  if (!Wire.available()) {
+    return false;
+  }
+  buf[0] = (uint8_t)Wire.read();
+  return true;
+}
+
+bool AK09918::writeByte(uint8_t addr,uint8_t reg ,uint8_t Value)
+{
+  Wire.beginTransmission(addr);
+  Wire.write(reg);
+  Wire.write(Value);
+  return Wire.endTransmission() == 0;
+}
+

AK09918.h

@@ -0,0 +1,149 @@
+/*
+    AK09918.h
+    A library for Grove - IMU 9DOF(ICM20600 + AK09918)
+
+    Copyright (c) 2018 seeed technology inc.
+    Website    : www.seeed.cc
+    Author     : Jerry Yip
+    Create Time: 2018-06
+    Version    : 0.1
+    Change Log :
+
+    The MIT License (MIT)
+
+    Permission is hereby granted, free of charge, to any person obtaining a copy
+    of this software and associated documentation files (the "Software"), to deal
+    in the Software without restriction, including without limitation the rights
+    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+    copies of the Software, and to permit persons to whom the Software is
+    furnished to do so, subject to the following conditions:
+
+    The above copyright notice and this permission notice shall be included in
+    all copies or substantial portions of the Software.
+
+    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+    THE SOFTWARE.
+*/
+
+
+
+
+#ifndef __IMU_9DOF_AK09918_H__
+#define __IMU_9DOF_AK09918_H__
+
+#include <Arduino.h>
+#include <Wire.h>
+
+/***************************************************************
+    AK09918 I2C Register
+ ***************************************************************/
+#define AK09918_I2C_ADDR    0x0c    // I2C address (Can't be changed)
+#define AK09918_WIA1        0x00    // Company ID
+#define AK09918_WIA2        0x01    // Device ID
+#define AK09918_RSV1        0x02    // Reserved 1
+#define AK09918_RSV2        0x03    // Reserved 2
+#define AK09918_ST1         0x10    // DataStatus 1
+#define AK09918_HXL         0x11    // X-axis data 
+#define AK09918_HXH         0x12
+#define AK09918_HYL         0x13    // Y-axis data
+#define AK09918_HYH         0x14
+#define AK09918_HZL         0x15    // Z-axis data
+#define AK09918_HZH         0x16
+#define AK09918_TMPS        0x17    // Dummy
+#define AK09918_ST2         0x18    // Datastatus 2
+#define AK09918_CNTL1       0x30    // Dummy
+#define AK09918_CNTL2       0x31    // Control settings
+#define AK09918_CNTL3       0x32    // Control settings
+
+#define AK09918_SRST_BIT    0x01    // Soft Reset
+#define AK09918_HOFL_BIT    0x08    // Sensor Over Flow
+#define AK09918_DOR_BIT     0x02    // Data Over Run
+#define AK09918_DRDY_BIT    0x01    // Data Ready
+
+// #define AK09918_MEASURE_PERIOD 9    // Must not be changed
+// AK09918 has following seven operation modes:
+// (1) Power-down mode: AK09918 doesn't measure
+// (2) Single measurement mode: measure when you call any getData() function
+// (3) Continuous measurement mode 1: 10Hz, measure 10 times per second,
+// (4) Continuous measurement mode 2: 20Hz, measure 20 times per second,
+// (5) Continuous measurement mode 3: 50Hz, measure 50 times per second,
+// (6) Continuous measurement mode 4: 100Hz, measure 100 times per second,
+// (7) Self-test mode
+enum AK09918_mode_type_t {
+    AK09918_POWER_DOWN = 0x00,
+    AK09918_NORMAL = 0x01,
+    AK09918_CONTINUOUS_10HZ = 0x02,
+    AK09918_CONTINUOUS_20HZ = 0x04,
+    AK09918_CONTINUOUS_50HZ = 0x06,
+    AK09918_CONTINUOUS_100HZ = 0x08,
+    AK09918_SELF_TEST = 0x10, // ignored by switchMode() and initialize(), call selfTest() to use this mode
+};
+
+enum AK09918_err_type_t {
+    AK09918_ERR_OK = 0,                 // ok
+    AK09918_ERR_DOR = 1,                // data skipped
+    AK09918_ERR_NOT_RDY = 2,            // not ready
+    AK09918_ERR_TIMEOUT = 3,            // read/write timeout
+    AK09918_ERR_SELFTEST_FAILED = 4,    // self test failed
+    AK09918_ERR_OVERFLOW = 5,           // sensor overflow, means |x|+|y|+|z| >= 4912uT
+    AK09918_ERR_WRITE_FAILED = 6,       // fail to write
+    AK09918_ERR_READ_FAILED = 7,        // fail to read
+
+};
+
+typedef struct imu_st_sensor_data_tag
+{
+  short int s16X;
+  short int s16Y;
+  short int s16Z;
+}IMU_ST_SENSOR_DATA;
+
+class AK09918 {
+  public:
+    AK09918();
+
+    // default to AK09918_CONTINUOUS_10HZ mode
+    AK09918_err_type_t initialize(AK09918_mode_type_t mode = AK09918_NORMAL);
+    // At AK09918_CONTINUOUS_** mode, check if data is ready to read
+    AK09918_err_type_t isDataReady();
+    // At AK09918_CONTINUOUS_** mode, check if data is skipped
+    AK09918_err_type_t isDataSkip();
+    // Get magnet data in uT
+    AK09918_err_type_t getData(int16_t* axis_x, int16_t* axis_y, int16_t* axis_z);
+    // Get raw I2C magnet data
+    AK09918_err_type_t getRawData(int16_t* axis_x, int16_t* axis_y, int16_t* axis_z);
+
+
+    // Return the working mode of AK09918
+    AK09918_mode_type_t getMode();
+    // Switch the working mode of AK09918
+    AK09918_err_type_t switchMode(AK09918_mode_type_t mode);
+    // Start a self-test, if pass, return AK09918_ERR_OK
+    AK09918_err_type_t selfTest();
+    // Reset AK09918
+    AK09918_err_type_t reset();
+    // Get details of AK09918_err_type_t
+    String strError(AK09918_err_type_t err);
+    // Get device ID
+    uint16_t getDeviceID();
+
+
+
+  private:
+    uint8_t _getRawMode();
+    bool writeByte(uint8_t addr,uint8_t reg ,uint8_t Value);
+    bool readByte(uint8_t addr,uint8_t reg,uint8_t *buf);
+    bool readBytes(uint8_t addr,uint8_t reg,uint8_t num,uint8_t *buf);
+    uint8_t _addr;
+    AK09918_mode_type_t _mode;
+    uint8_t _buffer[16];
+
+};
+
+
+#endif // __IMU_9DOF_AK09918_H__

IMU.cpp

@@ -0,0 +1,359 @@
+#include "IMU.h"
+
+void calibrateMagn();
+void imuAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
+float invSqrt(float x);
+
+/******************************************************************************
+ * IMU module                                                                 *
+ ******************************************************************************/
+// #define S_SCL   33
+// #define S_SDA   32
+
+AK09918_err_type_t err;
+
+QMI8658 qmi8658_;
+AK09918 magnetometer_;
+
+int16_t offset_x = -12, offset_y = 0, offset_z = 0;
+int16_t x, y, z;
+// Find the magnetic declination at your location
+// http://www.magnetic-declination.com/
+double declination_shenzhen = -3.22;
+
+#define Kp 4.50f   // proportional gain governs rate of convergence to accelerometer/magnetometer
+#define Ki 1.0f    // integral gain governs rate of convergence of gyroscope biases
+
+float angles[3];
+float q0, q1, q2, q3; 
+float exInt = 0.0f;
+float eyInt = 0.0f;
+float ezInt = 0.0f;
+uint32_t lastFilterUpdateUs = 0;
+
+namespace {
+constexpr float kDegToRad = 0.01745329251994329577f;
+constexpr float kDefaultSampleDt = 0.01f;
+constexpr float kMaxSampleDt = 0.1f;
+
+float clampUnit(float value)
+{
+  if (value > 1.0f) {
+    return 1.0f;
+  }
+  if (value < -1.0f) {
+    return -1.0f;
+  }
+  return value;
+}
+
+void resetFilterState()
+{
+  q0 = 1.0f;
+  q1 = 0.0f;
+  q2 = 0.0f;
+  q3 = 0.0f;
+  exInt = 0.0f;
+  eyInt = 0.0f;
+  ezInt = 0.0f;
+  lastFilterUpdateUs = micros();
+}
+
+float getSampleDeltaSeconds()
+{
+  const uint32_t nowUs = micros();
+
+  if (lastFilterUpdateUs == 0) {
+    lastFilterUpdateUs = nowUs;
+    return kDefaultSampleDt;
+  }
+
+  const uint32_t elapsedUs = nowUs - lastFilterUpdateUs;
+  lastFilterUpdateUs = nowUs;
+
+  const float deltaSeconds = elapsedUs * 1.0e-6f;
+  if (deltaSeconds <= 0.0f || deltaSeconds > kMaxSampleDt) {
+    return kDefaultSampleDt;
+  }
+
+  return deltaSeconds;
+}
+
+bool normalizeVector(float *xAxis, float *yAxis, float *zAxis)
+{
+  const float normSquared = (*xAxis * *xAxis) + (*yAxis * *yAxis) + (*zAxis * *zAxis);
+  if (normSquared <= 1.0e-12f) {
+    return false;
+  }
+
+  const float norm = invSqrt(normSquared);
+  *xAxis *= norm;
+  *yAxis *= norm;
+  *zAxis *= norm;
+  return true;
+}
+}  // namespace
+
+void imuInit()
+{
+    // Wire.begin(S_SDA, S_SCL);
+    // Serial.begin(115200);
+   
+    if (qmi8658_.begin() == 0)
+	      Serial.println("qmi8658_init fail");
+
+    if (magnetometer_.initialize())
+        Serial.println("AK09918_init fail") ;
+    magnetometer_.switchMode(AK09918_CONTINUOUS_100HZ);
+    err = magnetometer_.isDataReady();
+    int retry_times = 0;
+    while (err != AK09918_ERR_OK) {
+        Serial.println(err);
+        Serial.println("Waiting Sensor");
+        delay(100);
+        magnetometer_.reset();
+        delay(100);
+        magnetometer_.switchMode(AK09918_CONTINUOUS_100HZ);
+        err = magnetometer_.isDataReady();
+        retry_times ++;
+        if (retry_times > 10) {
+          break;
+        }
+    }
+    // Serial.println("Start figure-8 calibration after 1 seconds.");
+    // delay(1000);
+    // calibrate(10000, &offset_x, &offset_y, &offset_z);
+    // calibrateMagn();
+    resetFilterState();
+}
+
+void imuDataGet(EulerAngles *pstAngles, 
+                IMU_ST_SENSOR_DATA_FLOAT *pstGyroRawData,
+                IMU_ST_SENSOR_DATA_FLOAT *pstAccelRawData,
+                IMU_ST_SENSOR_DATA *pstMagnRawData)
+{
+  if (pstAngles == nullptr || pstGyroRawData == nullptr ||
+      pstAccelRawData == nullptr || pstMagnRawData == nullptr) {
+    return;
+  }
+
+  float  acc[3] = {0.0f, 0.0f, 0.0f};
+  float  gyro[3] = {0.0f, 0.0f, 0.0f};
+  float MotionVal[9];
+
+  const AK09918_err_type_t magErr = magnetometer_.getData(&x, &y, &z);
+  if (magErr == AK09918_ERR_OVERFLOW) {
+    Serial.println("AK09918 overflow detected, keeping last valid magnetic sample.");
+  }
+
+  pstMagnRawData->s16X = x- offset_x;
+  pstMagnRawData->s16Y = y- offset_y;
+  pstMagnRawData->s16Z = z- offset_z;
+
+  // qmi8658_.GetEulerAngles(&pstAngles->pitch,&pstAngles->roll,&pstAngles->yaw,acc,gyro);
+  qmi8658_.read_sensor_data(acc,gyro);
+
+  // pstAngles->roll = atan2((float)acc[1], (float)acc[2]);
+  // pstAngles->pitch = atan2(-(float)acc[0], sqrt((float)(acc[1] * acc[1]) + (float)(acc[2] * acc[2])));
+
+  // double Xheading = pstMagnRawData->s16X * cos(pstAngles->pitch) + pstMagnRawData->s16Y * sin(pstAngles->roll) * sin(pstAngles->pitch) + pstMagnRawData->s16Z * cos(pstAngles->roll) * sin(pstAngles->pitch);
+  // double Yheading = pstMagnRawData->s16Y * cos(pstAngles->roll) - pstMagnRawData->s16Z * sin(pstAngles->pitch);
+  
+  // pstAngles->yaw = /*180 + */57.3 * atan2(Yheading, Xheading) + declination_shenzhen;
+
+  // pstAngles->roll = atan2((float)acc[1], (float)acc[2]) * 57.3;
+  // pstAngles->pitch = atan2(-(float)acc[0], sqrt((float)(acc[1] * acc[1]) + (float)(acc[2] * acc[2]))) * 57.3;
+  MotionVal[0]=gyro[0];
+  MotionVal[1]=gyro[1];
+  MotionVal[2]=gyro[2];
+  MotionVal[3]=acc[0];
+  MotionVal[4]=acc[1];
+  MotionVal[5]=acc[2];
+  MotionVal[6]=pstMagnRawData->s16X;
+  MotionVal[7]=pstMagnRawData->s16Y;
+  MotionVal[8]=pstMagnRawData->s16Z;
+
+  imuAHRSupdate((float)MotionVal[0] * kDegToRad, (float)MotionVal[1] * kDegToRad, (float)MotionVal[2] * kDegToRad,
+                (float)MotionVal[3], (float)MotionVal[4], (float)MotionVal[5], 
+                (float)MotionVal[6], (float)MotionVal[7], MotionVal[8]);
+
+  pstAngles->pitch = asinf(clampUnit(-2.0f * q1 * q3 + 2.0f * q0 * q2)) * 57.2957795f;
+  pstAngles->roll = atan2f(2.0f * q2 * q3 + 2.0f * q0 * q1,
+                           -2.0f * q1 * q1 - 2.0f * q2 * q2 + 1.0f) * 57.2957795f;
+  pstAngles->yaw = atan2f(-2.0f * q1 * q2 - 2.0f * q0 * q3,
+                          2.0f * q2 * q2 + 2.0f * q3 * q3 - 1.0f) * 57.2957795f;
+
+  pstGyroRawData->X = gyro[0];
+  pstGyroRawData->Y = gyro[1];
+  pstGyroRawData->Z = gyro[2];
+
+  pstAccelRawData->X = acc[0];
+  pstAccelRawData->Y = acc[1];
+  pstAccelRawData->Z = acc[2];
+
+  return;  
+}
+
+bool imuRecalibrate()
+{
+  qmi8658_.autoOffsets();
+  resetFilterState();
+  return true;
+}
+
+void imuGetMagnOffsets(IMU_ST_SENSOR_DATA *pstMagnOffset)
+{
+  if (pstMagnOffset == nullptr) {
+    return;
+  }
+
+  pstMagnOffset->s16X = offset_x;
+  pstMagnOffset->s16Y = offset_y;
+  pstMagnOffset->s16Z = offset_z;
+}
+
+void imuSetMagnOffsets(int16_t inputX, int16_t inputY, int16_t inputZ)
+{
+  offset_x = inputX;
+  offset_y = inputY;
+  offset_z = inputZ;
+  resetFilterState();
+}
+
+float imuGetTemperature()
+{
+  return qmi8658_.read_temperature();
+}
+
+void imuAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) 
+{
+  const float sampleDt = getSampleDeltaSeconds();
+  const float halfT = 0.5f * sampleDt;
+  float hx = 0.0f, hy = 0.0f, hz = 0.0f, bx = 0.0f, bz = 0.0f;
+  float vx = 0.0f, vy = 0.0f, vz = 0.0f, wx = 0.0f, wy = 0.0f, wz = 0.0f;
+  float ex = 0.0f, ey = 0.0f, ez = 0.0f;
+
+  float q0q0 = q0 * q0;
+  float q0q1 = q0 * q1;
+  float q0q2 = q0 * q2;
+  float q0q3 = q0 * q3;
+  float q1q1 = q1 * q1;
+  float q1q2 = q1 * q2;
+  float q1q3 = q1 * q3;
+  float q2q2 = q2 * q2;   
+  float q2q3 = q2 * q3;
+  float q3q3 = q3 * q3;          
+
+  const bool accelValid = normalizeVector(&ax, &ay, &az);
+  const bool magnValid = normalizeVector(&mx, &my, &mz);
+
+  if (accelValid) {
+    vx = 2.0f * (q1q3 - q0q2);
+    vy = 2.0f * (q0q1 + q2q3);
+    vz = q0q0 - q1q1 - q2q2 + q3q3;
+
+    ex += (ay * vz - az * vy);
+    ey += (az * vx - ax * vz);
+    ez += (ax * vy - ay * vx);
+  }
+
+  if (magnValid) {
+    hx = 2.0f * mx * (0.5f - q2q2 - q3q3) + 2.0f * my * (q1q2 - q0q3) + 2.0f * mz * (q1q3 + q0q2);
+    hy = 2.0f * mx * (q1q2 + q0q3) + 2.0f * my * (0.5f - q1q1 - q3q3) + 2.0f * mz * (q2q3 - q0q1);
+    hz = 2.0f * mx * (q1q3 - q0q2) + 2.0f * my * (q2q3 + q0q1) + 2.0f * mz * (0.5f - q1q1 - q2q2);
+    bx = sqrtf((hx * hx) + (hy * hy));
+    bz = hz;
+
+    wx = 2.0f * bx * (0.5f - q2q2 - q3q3) + 2.0f * bz * (q1q3 - q0q2);
+    wy = 2.0f * bx * (q1q2 - q0q3) + 2.0f * bz * (q0q1 + q2q3);
+    wz = 2.0f * bx * (q0q2 + q1q3) + 2.0f * bz * (0.5f - q1q1 - q2q2);
+
+    ex += (my * wz - mz * wy);
+    ey += (mz * wx - mx * wz);
+    ez += (mx * wy - my * wx);
+  }
+
+  if (accelValid || magnValid) {
+    exInt += ex * Ki * sampleDt;
+    eyInt += ey * Ki * sampleDt;
+    ezInt += ez * Ki * sampleDt;
+
+    gx = gx + Kp * ex + exInt;
+    gy = gy + Kp * ey + eyInt;
+    gz = gz + Kp * ez + ezInt;
+  }
+
+  const float q0Prev = q0;
+  const float q1Prev = q1;
+  const float q2Prev = q2;
+  const float q3Prev = q3;
+
+  q0 = q0Prev + (-q1Prev * gx - q2Prev * gy - q3Prev * gz) * halfT;
+  q1 = q1Prev + (q0Prev * gx + q2Prev * gz - q3Prev * gy) * halfT;
+  q2 = q2Prev + (q0Prev * gy - q1Prev * gz + q3Prev * gx) * halfT;
+  q3 = q3Prev + (q0Prev * gz + q1Prev * gy - q2Prev * gx) * halfT;
+
+  const float normSquared = q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3;
+  if (normSquared <= 1.0e-12f) {
+    resetFilterState();
+    return;
+  }
+
+  const float norm = invSqrt(normSquared);
+  q0 = q0 * norm;
+  q1 = q1 * norm;
+  q2 = q2 * norm;
+  q3 = q3 * norm;
+}
+
+float invSqrt(float x) 
+{
+  if (x <= 0.0f) {
+    return 0.0f;
+  }
+
+  float halfx = 0.5f * x;
+  union {
+    float f;
+    uint32_t i;
+  } conv = { x };
+
+  conv.i = 0x5f3759df - (conv.i >> 1);
+
+  float y = conv.f;
+  y = y * (1.5f - (halfx * y * y));
+
+  return y;
+}
+
+void calibrateMagn(void)
+{
+  int16_t temp[9];
+  Serial.printf("keep 10dof-imu device horizontal and it will read x y z axis offset value after 4 seconds\n");
+  delay(4000);
+  Serial.printf("start read all axises offset value\n");
+  magnetometer_.getData(&x, &y, &z);
+  temp[0] = x;
+  temp[1] = y;
+  temp[2] = z;
+  
+  Serial.printf("rotate z axis 180 degrees and it will read all axises offset value after 4 seconds\n");
+  delay(4000);
+  Serial.printf("start read all axises offset value\n");
+  magnetometer_.getData(&x, &y, &z);
+  temp[3] = x;
+  temp[4] = y;
+  temp[5] = z;
+
+  Serial.printf("flip 10dof-imu device and keep it horizontal and it will read all axises offset value after 4 seconds\n");
+  delay(4000);
+  Serial.printf("start read all axises offset value\n");
+  magnetometer_.getData(&x, &y, &z);
+  temp[6] = x;
+  temp[7] = y;
+  temp[8] = z;
+
+  offset_x = (temp[0]+temp[3])/2;
+  offset_y = (temp[1]+temp[4])/2;
+  offset_z = (temp[5]+temp[8])/2;
+}

IMU.h

@@ -0,0 +1,35 @@
+#ifndef _IMU_H_
+#define _IMU_H_
+
+#include "AK09918.h"
+#include "QMI8658.h"
+#include <stdio.h>
+#include <math.h>
+
+typedef struct imu_st_angles_data_tag
+{
+  float fYaw;
+  float fPitch;
+  float fRoll;
+}IMU_ST_ANGLES_DATA;
+
+typedef struct imu_st_sensor_data_float
+{
+  float X;
+  float Y;
+  float Z;
+}IMU_ST_SENSOR_DATA_FLOAT;
+
+void imuInit();
+void imuDataGet(EulerAngles *pstAngles, 
+                IMU_ST_SENSOR_DATA_FLOAT *pstGyroRawData,
+                IMU_ST_SENSOR_DATA_FLOAT *pstAccelRawData,
+                IMU_ST_SENSOR_DATA *pstMagnRawData); 
+bool imuRecalibrate();
+void imuGetMagnOffsets(IMU_ST_SENSOR_DATA *pstMagnOffset);
+void imuSetMagnOffsets(int16_t offsetX, int16_t offsetY, int16_t offsetZ);
+float imuGetTemperature();
+void imuAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
+float invSqrt(float x);
+
+#endif

IMU_ctrl.h

@@ -0,0 +1,103 @@
+#include"IMU.h"
+
+// define GPIOs for IIC.
+EulerAngles stAngles;
+IMU_ST_SENSOR_DATA_FLOAT stGyroRawData;
+IMU_ST_SENSOR_DATA_FLOAT stAccelRawData;
+IMU_ST_SENSOR_DATA stMagnRawData;
+float temp;
+
+
+void imu_init() {
+	imuInit();
+}
+
+
+void updateIMUData() {
+		imuDataGet( &stAngles, &stGyroRawData, &stAccelRawData, &stMagnRawData);
+		temp = imuGetTemperature();
+
+		ax = stAccelRawData.X;
+		ay = stAccelRawData.Y;
+	az = stAccelRawData.Z;
+
+	mx = stMagnRawData.s16X;
+	my = stMagnRawData.s16Y;
+	mz = stMagnRawData.s16Z;
+
+	gx = stGyroRawData.X;
+	gy = stGyroRawData.Y;
+		gz = stGyroRawData.Z;
+
+		icm_roll = stAngles.roll;
+		icm_pitch = stAngles.pitch;
+		icm_yaw = stAngles.yaw;
+		icm_temp = temp;
+		last_imu_update = millis();
+}
+
+
+void imuCalibration() {
+		const bool calibrationOk = imuRecalibrate();
+		updateIMUData();
+
+		jsonInfoHttp.clear();
+		jsonInfoHttp["T"] = FEEDBACK_IMU_DATA;
+		jsonInfoHttp["status"] = calibrationOk ? 1 : 0;
+		jsonInfoHttp["info"] = calibrationOk ? "IMU calibration finished." : "IMU calibration failed.";
+		jsonInfoHttp["r"] = icm_roll;
+		jsonInfoHttp["p"] = icm_pitch;
+		jsonInfoHttp["y"] = icm_yaw;
+
+		String getInfoJsonString;
+		serializeJson(jsonInfoHttp, getInfoJsonString);
+		Serial.println(getInfoJsonString);
+}
+
+
+void getIMUData() {
+	jsonInfoHttp.clear();
+	jsonInfoHttp["T"] = FEEDBACK_IMU_DATA;
+
+	jsonInfoHttp["r"] = icm_roll;
+	jsonInfoHttp["p"] = icm_pitch;
+	jsonInfoHttp["y"] = icm_yaw;
+
+	jsonInfoHttp["ax"] = ax;
+	jsonInfoHttp["ay"] = ay;
+	jsonInfoHttp["az"] = az;
+
+	jsonInfoHttp["gx"] = gx;
+	jsonInfoHttp["gy"] = gy;
+	jsonInfoHttp["gz"] = gz;
+
+	jsonInfoHttp["mx"] = mx;
+	jsonInfoHttp["my"] = my;
+	jsonInfoHttp["mz"] = mz;
+
+	jsonInfoHttp["temp"] = temp;
+
+	String getInfoJsonString;
+	serializeJson(jsonInfoHttp, getInfoJsonString);
+	Serial.println(getInfoJsonString);
+}
+
+void getIMUOffset() {
+		IMU_ST_SENSOR_DATA offsetData;
+		imuGetMagnOffsets(&offsetData);
+
+		jsonInfoHttp.clear();
+		jsonInfoHttp["T"] = CMD_GET_IMU_OFFSET;
+		jsonInfoHttp["x"] = offsetData.s16X;
+		jsonInfoHttp["y"] = offsetData.s16Y;
+		jsonInfoHttp["z"] = offsetData.s16Z;
+
+		String getInfoJsonString;
+		serializeJson(jsonInfoHttp, getInfoJsonString);
+		Serial.println(getInfoJsonString);
+}
+
+void setIMUOffset(int16_t inputX, int16_t inputY, int16_t inputZ) {
+		imuSetMagnOffsets(inputX, inputY, inputZ);
+		getIMUOffset();
+}

QMI8658.cpp

@@ -0,0 +1,559 @@
+#include "QMI8658.h"
+
+#include <Wire.h>
+#include <math.h>
+
+#define QMI8658_UINT_MG_DPS
+//#define M_PI			(3.14159265358979323846f)
+#define ONE_G			(9.807f)
+
+
+static qmi8658_state g_imu;
+
+namespace {
+uint8_t qmi8658_address()
+{
+	return g_imu.slave ? g_imu.slave : QMI8658_ADDR;
+}
+}
+
+
+void QMI8658::write_reg(uint8_t reg,uint8_t value)
+{
+  Wire.beginTransmission(qmi8658_address());
+  Wire.write(reg);
+  Wire.write(value);
+  last_status = Wire.endTransmission();
+}
+
+uint8_t QMI8658::read_reg(uint8_t reg)
+{
+	uint8_t ret = 0;
+	unsigned int retry = 0;
+
+	while(retry++ < 5)
+	{
+	    Wire.beginTransmission(qmi8658_address());
+	    Wire.write(reg);
+	    last_status = Wire.endTransmission(false);
+	    if (last_status != 0) {
+	    	continue;
+	    }
+	    if (Wire.requestFrom((int)qmi8658_address(), 1) != 1) {
+	    	continue;
+	    }
+	    if (Wire.available()) {
+	    	ret = (uint8_t)Wire.read();
+	    	break;
+	    }
+	}
+	return ret;
+}
+
+uint16_t QMI8658::readWord_reg(uint8_t reg)
+{
+	uint8_t retH=0;
+  uint8_t retL=0;
+
+  Wire.beginTransmission(qmi8658_address());
+  Wire.write(reg);
+  last_status = Wire.endTransmission(false);
+  if (last_status != 0) {
+  	return 0;
+  }
+  if (Wire.requestFrom((int)qmi8658_address(), 2) != 2) {
+  	return 0;
+  }
+  if (Wire.available() < 2) {
+  	return 0;
+  }
+  retL = (uint8_t)Wire.read();
+  retH = (uint8_t)Wire.read();
+
+	return ((retH << 8) | retL);
+}
+
+
+void QMI8658::read_sensor_data(float acc[3], float gyro[3])
+{
+	unsigned char	buf_reg[12];
+	short 			raw_acc_xyz[3];
+	short 			raw_gyro_xyz[3];
+
+	raw_acc_xyz[0] = (short)((unsigned short)( readWord_reg(Qmi8658Register_Ax_L) ));
+	raw_acc_xyz[1] = (short)((unsigned short)( readWord_reg(Qmi8658Register_Ay_L) ));
+	raw_acc_xyz[2] = (short)((unsigned short)( readWord_reg(Qmi8658Register_Az_L) ));
+
+	raw_gyro_xyz[0] = (short)((unsigned short)( readWord_reg(Qmi8658Register_Gx_L) ));
+	raw_gyro_xyz[1] = (short)((unsigned short)( readWord_reg(Qmi8658Register_Gy_L) ));
+	raw_gyro_xyz[2] = (short)((unsigned short)( readWord_reg(Qmi8658Register_Gz_L) ));
+
+#if defined(QMI8658_UINT_MG_DPS)
+	// mg
+  // Serial.println("mg");
+	acc[0] = (float)(raw_acc_xyz[0]*1000.0f)/g_imu.ssvt_a - TempAcc.X_Off_Err;
+	acc[1] = (float)(raw_acc_xyz[1]*1000.0f)/g_imu.ssvt_a - TempAcc.Y_Off_Err;
+	acc[2] = (float)(raw_acc_xyz[2]*1000.0f)/g_imu.ssvt_a - TempAcc.Z_Off_Err;
+#else
+	// m/s2
+  // Serial.println("m/s2");
+	acc[0] = (float)(raw_acc_xyz[0]*ONE_G)/g_imu.ssvt_a;
+	acc[1] = (float)(raw_acc_xyz[1]*ONE_G)/g_imu.ssvt_a;
+	acc[2] = (float)(raw_acc_xyz[2]*ONE_G)/g_imu.ssvt_a;
+#endif
+
+#if defined(QMI8658_UINT_MG_DPS)
+	// dps
+  // Serial.println("dps");
+	gyro[0] = (float)(raw_gyro_xyz[0]*1.0f)/g_imu.ssvt_g - TempGyr.X_Off_Err;
+	gyro[1] = (float)(raw_gyro_xyz[1]*1.0f)/g_imu.ssvt_g - TempGyr.Y_Off_Err;
+	gyro[2] = (float)(raw_gyro_xyz[2]*1.0f)/g_imu.ssvt_g - TempGyr.Z_Off_Err;
+#else
+	// rad/s
+  // Serial.println("rad/s");
+	gyro[0] = (float)(raw_gyro_xyz[0]*M_PI)/(g_imu.ssvt_g*180);		// *pi/180
+	gyro[1] = (float)(raw_gyro_xyz[1]*M_PI)/(g_imu.ssvt_g*180);
+	gyro[2] = (float)(raw_gyro_xyz[2]*M_PI)/(g_imu.ssvt_g*180);
+#endif
+}
+
+void QMI8658::read_acc(float acc[3])
+{
+	unsigned char	buf_reg[12];
+	short 		raw_acc_xyz[3];
+
+	raw_acc_xyz[0] = (short)((unsigned short)( readWord_reg(Qmi8658Register_Ax_L) ));
+	raw_acc_xyz[1] = (short)((unsigned short)( readWord_reg(Qmi8658Register_Ay_L) ));
+	raw_acc_xyz[2] = (short)((unsigned short)( readWord_reg(Qmi8658Register_Az_L) ));
+
+#if defined(QMI8658_UINT_MG_DPS)
+	// mg
+	acc[0] = (float)(raw_acc_xyz[0]*1000.0f)/g_imu.ssvt_a;
+	acc[1] = (float)(raw_acc_xyz[1]*1000.0f)/g_imu.ssvt_a;
+	acc[2] = (float)(raw_acc_xyz[2]*1000.0f)/g_imu.ssvt_a;
+#else
+	// m/s2
+  // Serial.println("m/s2");
+	acc[0] = (float)(raw_acc_xyz[0]*ONE_G)/g_imu.ssvt_a;
+	acc[1] = (float)(raw_acc_xyz[1]*ONE_G)/g_imu.ssvt_a;
+	acc[2] = (float)(raw_acc_xyz[2]*ONE_G)/g_imu.ssvt_a;
+#endif
+}
+
+void QMI8658::read_gyro(float gyro[3])
+{
+	unsigned char	buf_reg[12];
+	short 		raw_gyro_xyz[3];
+
+	raw_gyro_xyz[0] = (short)((unsigned short)( readWord_reg(Qmi8658Register_Gx_L) ));
+	raw_gyro_xyz[1] = (short)((unsigned short)( readWord_reg(Qmi8658Register_Gy_L) ));
+	raw_gyro_xyz[2] = (short)((unsigned short)( readWord_reg(Qmi8658Register_Gz_L) ));
+
+#if defined(QMI8658_UINT_MG_DPS)
+	// dps
+	gyro[0] = (float)(raw_gyro_xyz[0]*1.0f)/g_imu.ssvt_g;
+	gyro[1] = (float)(raw_gyro_xyz[1]*1.0f)/g_imu.ssvt_g;
+	gyro[2] = (float)(raw_gyro_xyz[2]*1.0f)/g_imu.ssvt_g;
+#else
+	// rad/s
+  // Serial.println("rad/s");
+	gyro[0] = (float)(raw_gyro_xyz[0]*M_PI)/(g_imu.ssvt_g*180);		// *pi/180
+	gyro[1] = (float)(raw_gyro_xyz[1]*M_PI)/(g_imu.ssvt_g*180);
+	gyro[2] = (float)(raw_gyro_xyz[2]*M_PI)/(g_imu.ssvt_g*180);
+#endif
+}
+
+float QMI8658::read_temperature()
+{
+	const int16_t raw_temp = (int16_t)((unsigned short)(readWord_reg(Qmi8658Register_Tempearture_L)));
+	return raw_temp / 256.0f;
+}
+
+
+
+void QMI8658::axis_convert(float data_a[3], float data_g[3], int layout)
+{
+	float raw[3],raw_g[3];
+
+	raw[0] = data_a[0];
+	raw[1] = data_a[1];
+	//raw[2] = data[2];
+	raw_g[0] = data_g[0];
+	raw_g[1] = data_g[1];
+	//raw_g[2] = data_g[2];
+
+	if(layout >=4 && layout <= 7)
+	{
+		data_a[2] = -data_a[2];
+		data_g[2] = -data_g[2];
+	}
+
+	if(layout%2)
+	{
+		data_a[0] = raw[1];
+		data_a[1] = raw[0];
+
+		data_g[0] = raw_g[1];
+		data_g[1] = raw_g[0];
+	}
+	else
+	{
+		data_a[0] = raw[0];
+		data_a[1] = raw[1];
+
+		data_g[0] = raw_g[0];
+		data_g[1] = raw_g[1];
+	}
+
+	if((layout==1)||(layout==2)||(layout==4)||(layout==7))
+	{
+		data_a[0] = -data_a[0];
+		data_g[0] = -data_g[0];
+	}
+	if((layout==2)||(layout==3)||(layout==6)||(layout==7))
+	{
+		data_a[1] = -data_a[1];
+		data_g[1] = -data_g[1];
+	}
+}
+
+
+
+void QMI8658::read_xyz(float acc[3], float gyro[3])
+{
+	unsigned char	status;
+	unsigned char data_ready = 0;
+
+#if defined(QMI8658_SYNC_SAMPLE_MODE)
+	status = read_reg(Qmi8658Register_StatusInt);
+	if(status&0x01)
+	{
+		data_ready = 1;
+		delayMicroseconds(6);
+	}
+#else
+	status = read_reg(Qmi8658Register_Status0);
+	if(status&0x03)
+	{
+		data_ready = 1;
+	}
+#endif
+	if(data_ready)
+	{
+		read_sensor_data(acc, gyro);
+		axis_convert(acc, gyro, 0);
+	#if defined(QMI8658_USE_CALI)
+		qmi8658_data_cali(1, acc);
+		qmi8658_data_cali(2, gyro);
+#endif
+		g_imu.imu[0] = acc[0];
+		g_imu.imu[1] = acc[1];
+		g_imu.imu[2] = acc[2];
+		g_imu.imu[3] = gyro[0];
+		g_imu.imu[4] = gyro[1];
+		g_imu.imu[5] = gyro[2];
+	}
+	else
+	{
+		acc[0] = g_imu.imu[0];
+		acc[1] = g_imu.imu[1];
+		acc[2] = g_imu.imu[2];
+		gyro[0] = g_imu.imu[3];
+		gyro[1] = g_imu.imu[4];
+		gyro[2] = g_imu.imu[5];
+		Serial.print("data ready fail!\n");
+	}
+}
+
+
+
+void QMI8658::config_acc(enum qmi8658_AccRange range, enum qmi8658_AccOdr odr, enum qmi8658_LpfConfig lpfEnable, enum qmi8658_StConfig stEnable)
+{
+	unsigned char ctl_dada;
+
+	switch(range)
+	{
+		case Qmi8658AccRange_2g:
+			g_imu.ssvt_a = (1<<14);
+			break;
+		case Qmi8658AccRange_4g:
+			g_imu.ssvt_a = (1<<13);
+			break;
+		case Qmi8658AccRange_8g:
+			g_imu.ssvt_a = (1<<12);
+			break;
+		case Qmi8658AccRange_16g:
+			g_imu.ssvt_a = (1<<11);
+			break;
+		default:
+			range = Qmi8658AccRange_8g;
+			g_imu.ssvt_a = (1<<12);
+	}
+	if(stEnable == Qmi8658St_Enable)
+		ctl_dada = (unsigned char)range|(unsigned char)odr|0x80;
+	else
+		ctl_dada = (unsigned char)range|(unsigned char)odr;
+
+	write_reg(Qmi8658Register_Ctrl2, ctl_dada);
+// set LPF & HPF
+	ctl_dada = read_reg(Qmi8658Register_Ctrl5);
+	ctl_dada &= 0xf0;
+	if(lpfEnable == Qmi8658Lpf_Enable)
+	{
+		ctl_dada |= A_LSP_MODE_3;
+		ctl_dada |= 0x01;
+	}
+	else
+	{
+		ctl_dada &= ~0x01;
+	}
+	//ctl_dada = 0x00;
+	write_reg(Qmi8658Register_Ctrl5,ctl_dada);
+// set LPF & HPF
+}
+
+void QMI8658::config_gyro(enum qmi8658_GyrRange range, enum qmi8658_GyrOdr odr, enum qmi8658_LpfConfig lpfEnable, enum qmi8658_StConfig stEnable)
+{
+	// Set the CTRL3 register to configure dynamic range and ODR
+	unsigned char ctl_dada;
+
+	// Store the scale factor for use when processing raw data
+	switch (range)
+	{
+		case Qmi8658GyrRange_16dps:
+			g_imu.ssvt_g = 2048;
+			break;
+		case Qmi8658GyrRange_32dps:
+			g_imu.ssvt_g = 1024;
+			break;
+		case Qmi8658GyrRange_64dps:
+			g_imu.ssvt_g = 512;
+			break;
+		case Qmi8658GyrRange_128dps:
+			g_imu.ssvt_g = 256;
+			break;
+		case Qmi8658GyrRange_256dps:
+			g_imu.ssvt_g = 128;
+			break;
+		case Qmi8658GyrRange_512dps:
+			g_imu.ssvt_g = 64;
+			break;
+		case Qmi8658GyrRange_1024dps:
+			g_imu.ssvt_g = 32;
+			break;
+		case Qmi8658GyrRange_2048dps:
+			g_imu.ssvt_g = 16;
+			break;
+//		case Qmi8658GyrRange_4096dps:
+//			g_imu.ssvt_g = 8;
+//			break;
+		default:
+			range = Qmi8658GyrRange_512dps;
+			g_imu.ssvt_g = 64;
+			break;
+	}
+
+	if(stEnable == Qmi8658St_Enable)
+		ctl_dada = (unsigned char)range|(unsigned char)odr|0x80;
+	else
+		ctl_dada = (unsigned char)range | (unsigned char)odr;
+	write_reg(Qmi8658Register_Ctrl3, ctl_dada);
+
+// Conversion from degrees/s to rad/s if necessary
+// set LPF & HPF
+	ctl_dada = read_reg(Qmi8658Register_Ctrl5);
+	ctl_dada &= 0x0f;
+	if(lpfEnable == Qmi8658Lpf_Enable)
+	{
+		ctl_dada |= G_LSP_MODE_3;
+		ctl_dada |= 0x10;
+	}
+	else
+	{
+		ctl_dada &= ~0x10;
+	}
+	//ctl_dada = 0x00;
+	write_reg(Qmi8658Register_Ctrl5,ctl_dada);
+// set LPF & HPF
+}
+
+void QMI8658::enableSensors(unsigned char enableFlags)
+{
+#if defined(QMI8658_SYNC_SAMPLE_MODE)
+	write_reg(Qmi8658Register_Ctrl7, enableFlags | 0x80);
+#elif defined(QMI8658_USE_FIFO)
+	//qmi8658_write_reg(Qmi8658Register_Ctrl7, enableFlags|QMI8658_DRDY_DISABLE);
+	write_reg(Qmi8658Register_Ctrl7, enableFlags);
+#else
+	write_reg(Qmi8658Register_Ctrl7, enableFlags);
+#endif
+	g_imu.cfg.enSensors = enableFlags&0x03;
+
+	delay(1);
+}
+
+void QMI8658::config_reg(unsigned char low_power)
+{
+	enableSensors(QMI8658_DISABLE_ALL);
+	if(low_power)
+	{
+		g_imu.cfg.enSensors = QMI8658_ACC_ENABLE;
+		g_imu.cfg.accRange = Qmi8658AccRange_8g;
+		g_imu.cfg.accOdr = Qmi8658AccOdr_LowPower_21Hz;
+		g_imu.cfg.gyrRange = Qmi8658GyrRange_1024dps;
+		g_imu.cfg.gyrOdr = Qmi8658GyrOdr_250Hz;
+	}
+	else
+	{
+		g_imu.cfg.enSensors = QMI8658_ACCGYR_ENABLE;
+		g_imu.cfg.accRange = Qmi8658AccRange_16g;
+		g_imu.cfg.accOdr = Qmi8658AccOdr_1000Hz;
+		g_imu.cfg.gyrRange = Qmi8658GyrRange_2048dps;
+		g_imu.cfg.gyrOdr = Qmi8658GyrOdr_1000Hz;
+	}
+
+	if(g_imu.cfg.enSensors & QMI8658_ACC_ENABLE)
+	{
+		config_acc(g_imu.cfg.accRange, g_imu.cfg.accOdr, Qmi8658Lpf_Disable, Qmi8658St_Disable);
+	}
+	if(g_imu.cfg.enSensors & QMI8658_GYR_ENABLE)
+	{
+		config_gyro(g_imu.cfg.gyrRange, g_imu.cfg.gyrOdr, Qmi8658Lpf_Disable, Qmi8658St_Disable);
+	}
+}
+
+unsigned char QMI8658::get_id(void)
+{
+	unsigned char qmi8658_chip_id = 0x00;
+	unsigned char qmi8658_revision_id = 0x00;
+	unsigned char qmi8658_slave[2] = {QMI8658_SLAVE_ADDR_L, QMI8658_SLAVE_ADDR_H};
+	int retry = 0;
+	unsigned char iCount = 0;
+	unsigned char firmware_id[3];
+	unsigned char uuid[6];
+	unsigned int uuid_low, uuid_high;
+
+	while(iCount<2)
+	{
+		g_imu.slave = qmi8658_slave[iCount];
+		retry = 0;
+		while((qmi8658_chip_id != 0x05)&&(retry++ < 5))
+		{
+			qmi8658_chip_id = read_reg(Qmi8658Register_WhoAmI);
+			Serial.printf("Qmi8658Register_WhoAmI = 0x%x\n", qmi8658_chip_id);
+		}
+		if(qmi8658_chip_id == 0x05)
+		{
+			qmi8658_on_demand_cali();
+
+			g_imu.cfg.ctrl8_value = 0xc0;
+			//QMI8658_INT1_ENABLE, QMI8658_INT2_ENABLE
+			write_reg(Qmi8658Register_Ctrl1, 0x60|QMI8658_INT2_ENABLE|QMI8658_INT1_ENABLE);
+			qmi8658_revision_id = read_reg(Qmi8658Register_Revision);
+			// qmi8658_read_reg(Qmi8658Register_firmware_id, firmware_id, 3);
+			// qmi8658_read_reg(Qmi8658Register_uuid, uuid, 6);
+			write_reg(Qmi8658Register_Ctrl7, 0x00);
+			write_reg(Qmi8658Register_Ctrl8, g_imu.cfg.ctrl8_value);
+			// uuid_low = (unsigned int)((unsigned int)(uuid[2]<<16)|(unsigned int)(uuid[1]<<8)|(uuid[0]));
+			// uuid_high = (unsigned int)((unsigned int)(uuid[5]<<16)|(unsigned int)(uuid[4]<<8)|(uuid[3]));
+			// qmi8658_log("qmi8658_init slave=0x%x Revision=0x%x\n", g_imu.slave, qmi8658_revision_id);
+			// qmi8658_log("Firmware ID[0x%x 0x%x 0x%x]\n", firmware_id[2], firmware_id[1],firmware_id[0]);
+			// qmi8658_log("UUID[0x%x %x]\n", uuid_high ,uuid_low);
+			break;
+		}
+		iCount++;
+	}
+
+	return qmi8658_chip_id;
+}
+
+
+void QMI8658::qmi8658_on_demand_cali(void)
+{
+	Serial.print("qmi8658_on_demand_cali start\n");
+	write_reg(Qmi8658Register_Reset, 0xb0);
+	delay(10);	// delay
+	write_reg(Qmi8658Register_Ctrl9, (unsigned char)qmi8658_Ctrl9_Cmd_On_Demand_Cali);
+	delay(2200);	// delay 2000ms above
+	write_reg(Qmi8658Register_Ctrl9, (unsigned char)qmi8658_Ctrl9_Cmd_NOP);
+	delay(100);	// delay
+	Serial.print("qmi8658_on_demand_cali done\n");
+}
+
+unsigned char QMI8658::begin(void)
+{
+	if(get_id() == 0x05)
+	{
+#if defined(QMI8658_USE_AMD)
+		qmi8658_config_amd();
+#endif
+#if defined(QMI8658_USE_PEDOMETER)
+		qmi8658_config_pedometer(125);
+		qmi8658_enable_pedometer(1);
+#endif
+		config_reg(0);
+		enableSensors(g_imu.cfg.enSensors);
+    Serial.println("Position your ICM20948 flat and don't move it - calibrating...");
+    delay(1000);
+    autoOffsets();
+    // Serial.print();
+		dump_reg();
+#if defined(QMI8658_USE_CALI)
+		memset(&g_cali, 0, sizeof(g_cali));
+#endif
+		return 1;
+	}
+	else
+	{
+		// Serial.print("qmi8658_init fail\n");
+		return 0;
+	}
+}
+
+
+void QMI8658::dump_reg(void)
+{
+	// unsigned char read_data[8];
+
+	// qmi8658_read_reg(Qmi8658Register_Ctrl1, read_data, 8);
+	// qmi8658_log("Ctrl1[0x%x]\nCtrl2[0x%x]\nCtrl3[0x%x]\nCtrl4[0x%x]\nCtrl5[0x%x]\nCtrl6[0x%x]\nCtrl7[0x%x]\nCtrl8[0x%x]\n",
+	// 				read_data[0],read_data[1],read_data[2],read_data[3],read_data[4],read_data[5],read_data[6],read_data[7]);
+}
+
+void QMI8658::autoOffsets(void){
+    float acc[3],gyro[3];
+
+    TempAcc.X_Off_Err = 0.0f;
+    TempAcc.Y_Off_Err = 0.0f;
+    TempAcc.Z_Off_Err = 0.0f;
+    TempGyr.X_Off_Err = 0.0f;
+    TempGyr.Y_Off_Err = 0.0f;
+    TempGyr.Z_Off_Err = 0.0f;
+
+    for(int i=0; i<50; i++){
+        QMI8658::read_acc(acc);
+        TempAcc.X_Off_Err += acc[0];
+        TempAcc.Y_Off_Err += acc[1];
+        TempAcc.Z_Off_Err += acc[2];
+        delay(10);
+    }
+    
+    TempAcc.X_Off_Err /= 50;
+    TempAcc.Y_Off_Err /= 50;
+    TempAcc.Z_Off_Err /= 50;
+    TempAcc.Z_Off_Err -= 1000.0f;
+    
+    for(int i=0; i<50; i++){
+        QMI8658::read_gyro(gyro);
+        TempGyr.X_Off_Err += gyro[0];
+        TempGyr.Y_Off_Err += gyro[1];
+        TempGyr.Z_Off_Err += gyro[2];
+        delay(1);
+    }
+    
+    TempGyr.X_Off_Err /= 50;
+    TempGyr.Y_Off_Err /= 50;
+    TempGyr.Z_Off_Err /= 50;
+    
+}
+

QMI8658.h

@@ -0,0 +1,102 @@
+/*
+ * @Description: QMI8658
+ * @Author: zjw
+ * @Date: 2022-10-24
+ * @LastEditTime: 2022-10-24
+ * @LastEditors: zjw
+ */
+
+#ifndef _QMI8658_H_
+#define _QMI8658_H_
+
+#include <Arduino.h>
+#include "QMI8658reg.h"
+
+typedef struct
+{
+	float roll;
+  float pitch;
+  float yaw ;
+} EulerAngles;
+
+typedef struct
+{
+  float X_Off_Err;
+  float Y_Off_Err;
+  float Z_Off_Err;
+}QMI8658_TypeDef_Off;
+
+class QMI8658
+{
+  uint8_t last_status; // status of last I2C transmission
+  uint8_t read_reg(uint8_t reg);
+
+  void write_reg(uint8_t reg,uint8_t value);
+
+public:
+  uint16_t readWord_reg(uint8_t reg);
+  // bool init(void);
+  // bool GetEulerAngles(float *pitch,float *roll, float *yaw,float acc[3],float gyro[3]);
+
+  void config_acc(enum qmi8658_AccRange range, enum qmi8658_AccOdr odr,
+                      enum qmi8658_LpfConfig lpfEnable, enum qmi8658_StConfig stEnable);
+  void config_gyro(enum qmi8658_GyrRange range, enum qmi8658_GyrOdr odr,
+                      enum qmi8658_LpfConfig lpfEnable, enum qmi8658_StConfig stEnable);
+
+  void read_sensor_data(float acc[3], float gyro[3]);
+  void read_acc(float acc[3]);
+  void read_gyro(float gyro[3]);
+  float read_temperature();
+  void read_xyz(float acc[3], float gyro[3]);
+  void axis_convert(float data_a[3], float data_g[3], int layout);
+  void config_reg(unsigned char low_power);
+  void enableSensors(unsigned char enableFlags);
+  unsigned char get_id(void);
+  unsigned char begin(void);
+  void dump_reg(void);
+  void qmi8658_on_demand_cali(void);
+  void autoOffsets(void);
+
+public:
+  int16_t ax, ay, az, gx, gy, gz;
+  float pith, roll, yaw;
+  unsigned long now, lastTime = 0;
+  float dt;      //微分时间
+  float agz = 0; //角度变量
+  long gzo = 0;  //陀螺仪偏移量
+  QMI8658_TypeDef_Off TempAcc = {0};//校准值
+  QMI8658_TypeDef_Off TempGyr = {0};
+};
+
+/*----------------------------------------------------------------------------------------------
+  QMI8658C UI Sensor Configuration Settings and Output Data
+*/
+///<Configuration Registers>
+#define QMI8658_ADDR 0X6B  //device address
+#define WHO_AM_I 0X00 //Device identifier
+#define CTRL1 0x02    //Serial Interface and Sensor Enable
+#define CTRL2 0x03    //Accelerometer Settings
+#define CTRL3 0x04    //Gyroscope Settings
+#define CTRL4 0X05    //Magnetometer Settings
+#define CTRL5 0X06    //Sensor Data Processing Settings
+#define CTRL7 0x08    //Enable Sensors and Configure Data Reads
+#define CTRL8 0X09    //Reserved – Special Settings
+
+///<Sensor Data Output Registers>
+#define AccX_L 0x35
+#define AccX_H 0x36
+#define AccY_L 0x37
+#define AccY_H 0x38
+#define AccZ_L 0x39
+#define AccZ_H 0x3A
+#define TEMP_L 0x33
+
+#define GyrX_L 0x3B
+#define GyrX_H 0x3C
+#define GyrY_L 0x3D
+#define GyrY_H 0x3E
+#define GyrZ_L 0x3F
+#define GyrZ_H 0x40
+// int16_t QMI8658C_readBytes(unsigned char tmp);
+//extern QMI8658C _QMI8658C;
+#endif

QMI8658reg.h

@@ -0,0 +1,344 @@
+
+#ifndef _QMI8658REG_H_
+#define _QMI8658REG_H_
+
+// #define QMI8658_USE_SPI
+//#define QMI8658_SYNC_SAMPLE_MODE
+//#define QMI8658_SOFT_SELFTEST
+//#define QMI8658_USE_CALI
+
+#define QMI8658_USE_FIFO
+//#define QMI8658_USE_AMD
+//#define QMI8658_USE_PEDOMETER
+
+
+#define QMI8658_SLAVE_ADDR_L			0x6a
+#define QMI8658_SLAVE_ADDR_H			0x6b
+
+#define QMI8658_DISABLE_ALL				(0x0)
+#define QMI8658_ACC_ENABLE				(0x1)
+#define QMI8658_GYR_ENABLE				(0x2)
+#define QMI8658_ACCGYR_ENABLE			(QMI8658_ACC_ENABLE | QMI8658_GYR_ENABLE)
+
+#define QMI8658_STATUS1_CMD_DONE		(0x01)
+#define QMI8658_STATUS1_WAKEUP_EVENT	(0x04)
+
+#define QMI8658_CTRL8_DATAVALID_EN		0x40		// bit6:1 int1, 0 int2
+#define QMI8658_CTRL8_PEDOMETER_EN		0x10
+#define QMI8658_CTRL8_SIGMOTION_EN		0x08
+#define QMI8658_CTRL8_NOMOTION_EN		0x04
+#define QMI8658_CTRL8_ANYMOTION_EN		0x02
+#define QMI8658_CTRL8_TAP_EN			0x01
+
+#define QMI8658_INT1_ENABLE				0x08
+#define QMI8658_INT2_ENABLE				0x10
+
+#define QMI8658_DRDY_DISABLE			0x20	// ctrl7
+
+#define QMI8658_FIFO_MAP_INT1			0x04	// ctrl1
+#define QMI8658_FIFO_MAP_INT2			~0x04	// ctrl1
+
+#define qmi8658_log		printf
+
+enum Qmi8658Register
+{
+	Qmi8658Register_WhoAmI = 0,
+	Qmi8658Register_Revision,
+	Qmi8658Register_Ctrl1,
+	Qmi8658Register_Ctrl2,
+	Qmi8658Register_Ctrl3,
+	Qmi8658Register_Ctrl4,
+	Qmi8658Register_Ctrl5,
+	Qmi8658Register_Ctrl6,
+	Qmi8658Register_Ctrl7,
+	Qmi8658Register_Ctrl8,
+	Qmi8658Register_Ctrl9,
+	Qmi8658Register_Cal1_L = 11,
+	Qmi8658Register_Cal1_H,
+	Qmi8658Register_Cal2_L,
+	Qmi8658Register_Cal2_H,
+	Qmi8658Register_Cal3_L,
+	Qmi8658Register_Cal3_H,
+	Qmi8658Register_Cal4_L,
+	Qmi8658Register_Cal4_H,
+	Qmi8658Register_FifoWmkTh = 19,
+	Qmi8658Register_FifoCtrl = 20,
+	Qmi8658Register_FifoCount = 21,
+	Qmi8658Register_FifoStatus = 22,
+	Qmi8658Register_FifoData = 23,
+	Qmi8658Register_StatusI2CM = 44,
+	Qmi8658Register_StatusInt = 45,
+	Qmi8658Register_Status0,
+	Qmi8658Register_Status1,
+	Qmi8658Register_Timestamp_L = 48,
+	Qmi8658Register_Timestamp_M,
+	Qmi8658Register_Timestamp_H,
+	Qmi8658Register_Tempearture_L = 51,
+	Qmi8658Register_Tempearture_H,
+	Qmi8658Register_Ax_L = 53,
+	Qmi8658Register_Ax_H,
+	Qmi8658Register_Ay_L,
+	Qmi8658Register_Ay_H,
+	Qmi8658Register_Az_L,
+	Qmi8658Register_Az_H,
+	Qmi8658Register_Gx_L = 59,
+	Qmi8658Register_Gx_H,
+	Qmi8658Register_Gy_L,
+	Qmi8658Register_Gy_H,
+	Qmi8658Register_Gz_L,
+	Qmi8658Register_Gz_H,
+	Qmi8658Register_Mx_L = 65,
+	Qmi8658Register_Mx_H,
+	Qmi8658Register_My_L,
+	Qmi8658Register_My_H,
+	Qmi8658Register_Mz_L,
+	Qmi8658Register_Mz_H,
+	Qmi8658Register_firmware_id = 73,
+	Qmi8658Register_uuid = 81,
+
+	Qmi8658Register_Pedo_L = 90,
+	Qmi8658Register_Pedo_M = 91,
+	Qmi8658Register_Pedo_H = 92,
+
+	Qmi8658Register_Reset = 96
+};
+
+enum qmi8658_Ois_Register
+{
+	qmi8658_OIS_Reg_Ctrl1 = 0x02,
+	qmi8658_OIS_Reg_Ctrl2,
+	qmi8658_OIS_Reg_Ctrl3,
+	qmi8658_OIS_Reg_Ctrl5	= 0x06,
+	qmi8658_OIS_Reg_Ctrl7   = 0x08,
+	qmi8658_OIS_Reg_StatusInt = 0x2D,
+	qmi8658_OIS_Reg_Status0   = 0x2E,
+	qmi8658_OIS_Reg_Ax_L  = 0x33,
+	qmi8658_OIS_Reg_Ax_H,
+	qmi8658_OIS_Reg_Ay_L,
+	qmi8658_OIS_Reg_Ay_H,
+	qmi8658_OIS_Reg_Az_L,
+	qmi8658_OIS_Reg_Az_H,
+
+	qmi8658_OIS_Reg_Gx_L  = 0x3B,
+	qmi8658_OIS_Reg_Gx_H,
+	qmi8658_OIS_Reg_Gy_L,
+	qmi8658_OIS_Reg_Gy_H,
+	qmi8658_OIS_Reg_Gz_L,
+	qmi8658_OIS_Reg_Gz_H,
+};
+
+enum qmi8658_Ctrl9Command
+{
+	qmi8658_Ctrl9_Cmd_NOP					= 0X00,
+	qmi8658_Ctrl9_Cmd_GyroBias				= 0X01,
+	qmi8658_Ctrl9_Cmd_Rqst_Sdi_Mod			= 0X03,
+	qmi8658_Ctrl9_Cmd_Rst_Fifo				= 0X04,
+	qmi8658_Ctrl9_Cmd_Req_Fifo				= 0X05,
+	qmi8658_Ctrl9_Cmd_I2CM_Write			= 0X06,
+	qmi8658_Ctrl9_Cmd_WoM_Setting			= 0x08,
+	qmi8658_Ctrl9_Cmd_AccelHostDeltaOffset	= 0x09,
+	qmi8658_Ctrl9_Cmd_GyroHostDeltaOffset	= 0x0A,
+	qmi8658_Ctrl9_Cmd_EnableExtReset		= 0x0B,
+	qmi8658_Ctrl9_Cmd_EnableTap				= 0x0C,
+	qmi8658_Ctrl9_Cmd_EnablePedometer		= 0x0D,
+	qmi8658_Ctrl9_Cmd_Motion				= 0x0E,
+	qmi8658_Ctrl9_Cmd_CopyUsid				= 0x10,
+	qmi8658_Ctrl9_Cmd_SetRpu				= 0x11,
+	qmi8658_Ctrl9_Cmd_On_Demand_Cali		= 0xA2,
+	qmi8658_Ctrl9_Cmd_Dbg_WoM_Data_Enable	= 0xF8
+};
+
+
+enum qmi8658_LpfConfig
+{
+	Qmi8658Lpf_Disable,
+	Qmi8658Lpf_Enable
+};
+
+enum qmi8658_HpfConfig
+{
+	Qmi8658Hpf_Disable,
+	Qmi8658Hpf_Enable
+};
+
+enum qmi8658_StConfig
+{
+	Qmi8658St_Disable,
+	Qmi8658St_Enable
+};
+
+enum qmi8658_LpfMode
+{
+	A_LSP_MODE_0 = 0x00<<1,
+	A_LSP_MODE_1 = 0x01<<1,
+	A_LSP_MODE_2 = 0x02<<1,
+	A_LSP_MODE_3 = 0x03<<1,
+
+	G_LSP_MODE_0 = 0x00<<5,
+	G_LSP_MODE_1 = 0x01<<5,
+	G_LSP_MODE_2 = 0x02<<5,
+	G_LSP_MODE_3 = 0x03<<5
+};
+
+enum qmi8658_AccRange
+{
+	Qmi8658AccRange_2g = 0x00 << 4,
+	Qmi8658AccRange_4g = 0x01 << 4,
+	Qmi8658AccRange_8g = 0x02 << 4,
+	Qmi8658AccRange_16g = 0x03 << 4
+};
+
+
+enum qmi8658_AccOdr
+{
+	Qmi8658AccOdr_8000Hz = 0x00,
+	Qmi8658AccOdr_4000Hz = 0x01,
+	Qmi8658AccOdr_2000Hz = 0x02,
+	Qmi8658AccOdr_1000Hz = 0x03,
+	Qmi8658AccOdr_500Hz = 0x04,
+	Qmi8658AccOdr_250Hz = 0x05,
+	Qmi8658AccOdr_125Hz = 0x06,
+	Qmi8658AccOdr_62_5Hz = 0x07,
+	Qmi8658AccOdr_31_25Hz = 0x08,
+	Qmi8658AccOdr_LowPower_128Hz = 0x0c,
+	Qmi8658AccOdr_LowPower_21Hz = 0x0d,
+	Qmi8658AccOdr_LowPower_11Hz = 0x0e,
+	Qmi8658AccOdr_LowPower_3Hz = 0x0f
+};
+
+enum qmi8658_GyrRange
+{
+	Qmi8658GyrRange_16dps = 0 << 4,
+	Qmi8658GyrRange_32dps = 1 << 4,
+	Qmi8658GyrRange_64dps = 2 << 4,
+	Qmi8658GyrRange_128dps = 3 << 4,
+	Qmi8658GyrRange_256dps = 4 << 4,
+	Qmi8658GyrRange_512dps = 5 << 4,
+	Qmi8658GyrRange_1024dps = 6 << 4,
+	Qmi8658GyrRange_2048dps = 7 << 4
+};
+
+/*!
+ * \brief Gyroscope output rate configuration.
+ */
+enum qmi8658_GyrOdr
+{
+	Qmi8658GyrOdr_8000Hz = 0x00,
+	Qmi8658GyrOdr_4000Hz = 0x01,
+	Qmi8658GyrOdr_2000Hz = 0x02,
+	Qmi8658GyrOdr_1000Hz = 0x03,
+	Qmi8658GyrOdr_500Hz	= 0x04,
+	Qmi8658GyrOdr_250Hz	= 0x05,
+	Qmi8658GyrOdr_125Hz	= 0x06,
+	Qmi8658GyrOdr_62_5Hz 	= 0x07,
+	Qmi8658GyrOdr_31_25Hz	= 0x08
+};
+
+enum qmi8658_AccUnit
+{
+	Qmi8658AccUnit_g,
+	Qmi8658AccUnit_ms2
+};
+
+enum qmi8658_GyrUnit
+{
+	Qmi8658GyrUnit_dps,
+	Qmi8658GyrUnit_rads
+};
+
+enum qmi8658_FifoMode
+{
+	qmi8658_Fifo_Bypass = 0,
+	qmi8658_Fifo_Fifo = 1,
+	qmi8658_Fifo_Stream = 2,
+	qmi8658_Fifo_StreamToFifo = 3
+};
+
+
+enum qmi8658_FifoWmkLevel
+{
+	qmi8658_Fifo_WmkEmpty = 		(0 << 4),
+	qmi8658_Fifo_WmkOneQuarter =	(1 << 4),
+	qmi8658_Fifo_WmkHalf =			(2 << 4),
+	qmi8658_Fifo_WmkThreeQuarters =	(3 << 4)
+};
+
+enum qmi8658_FifoSize
+{
+	qmi8658_Fifo_16 = (0 << 2),
+	qmi8658_Fifo_32 = (1 << 2),
+	qmi8658_Fifo_64 = (2 << 2),
+	qmi8658_Fifo_128 = (3 << 2)
+};
+
+enum qmi8658_Interrupt
+{
+	qmi8658_Int_none,
+	qmi8658_Int1,
+	qmi8658_Int2,
+
+	qmi8658_Int_total
+};
+
+enum qmi8658_InterruptState
+{
+	Qmi8658State_high = (1 << 7),
+	Qmi8658State_low  = (0 << 7)
+};
+
+#define QMI8658_CALI_DATA_NUM	200
+
+typedef struct qmi8658_cali
+{
+	float			acc_last[3];
+	float			acc[3];
+	float			acc_fix[3];
+	float			acc_bias[3];
+	float			acc_sum[3];
+
+	float			gyr_last[3];
+	float			gyr[3];
+	float			gyr_fix[3];
+	float			gyr_bias[3];
+	float			gyr_sum[3];
+
+	unsigned char	imu_static_flag;
+	unsigned char	acc_fix_flag;
+	unsigned char	gyr_fix_flag;
+	char			acc_fix_index;
+	unsigned char	gyr_fix_index;
+
+	unsigned char	acc_cali_flag;
+	unsigned char	gyr_cali_flag;
+	unsigned short	acc_cali_num;
+	unsigned short	gyr_cali_num;
+//    unsigned char	acc_avg_num;
+//    unsigned char	gyr_avg_num;
+} qmi8658_cali;
+
+typedef struct
+{
+	unsigned char			enSensors;
+	enum qmi8658_AccRange	accRange;
+	enum qmi8658_AccOdr		accOdr;
+	enum qmi8658_GyrRange	gyrRange;
+	enum qmi8658_GyrOdr		gyrOdr;
+	unsigned char			ctrl8_value;
+#if defined(QMI8658_USE_FIFO)
+	unsigned char			fifo_ctrl;
+#endif
+} qmi8658_config;
+
+typedef struct
+{
+	unsigned char	slave;
+	qmi8658_config	cfg;
+	unsigned short	ssvt_a;
+	unsigned short	ssvt_g;
+	unsigned int	timestamp;
+	unsigned int	step;
+	float			imu[6];
+} qmi8658_state;
+
+
+#endif

README.md

@@ -0,0 +1,13 @@
+# WAVE_ROVER
+
+Firmware project for the Wave Rover ESP32 platform.
+
+Current working version:
+
+- `1.0`
+
+Highlights in this repo:
+
+- IMU fixes and calibration improvements
+- Web UI updates
+- ESP32 Arduino sketch source for ongoing versioning

WAVE_ROVER_V1.0.ino

@@ -0,0 +1,267 @@
+#include <ArduinoJson.h>
+StaticJsonDocument<256> jsonCmdReceive;
+StaticJsonDocument<256> jsonInfoSend;
+StaticJsonDocument<512> jsonInfoHttp;
+
+// TaskHandle_t Pid_ctrl;
+
+#include <SCServo.h>
+#include <nvs_flash.h>
+#include <esp_system.h>
+#include <LittleFS.h>
+#include <WiFi.h>
+#include <WebServer.h>
+#include <esp_now.h>
+#include <nvs_flash.h>
+#include <Adafruit_SSD1306.h>
+#include <INA219_WE.h>
+#include <ESP32Encoder.h>
+#include <PID_v2.h>
+#include <SimpleKalmanFilter.h>
+#include <math.h>
+#include <Adafruit_ICM20X.h>
+#include <Adafruit_ICM20948.h>
+#include <Adafruit_Sensor.h>
+
+
+// functions for barrery info.
+#include "battery_ctrl.h"
+
+// functions for oled.
+#include "oled_ctrl.h"
+
+// config for ugv.
+#include "ugv_config.h"
+
+// functions for the leds of UGV.
+#include "ugv_led_ctrl.h"
+
+// functions for RoArm-M2 ctrl.
+#include "RoArm-M2_module.h"
+
+// functions for gimbal ctrl.
+#include "gimbal_module.h"
+
+// define json cmd.
+#include "json_cmd.h"
+
+// functions for IMU ctrl.
+#include "IMU_ctrl.h"
+
+// functions for movtion ctrl. 
+#include "movtion_module.h"
+
+// functions for editing the files in flash.
+#include "files_ctrl.h"
+
+// advance functions for ugv ctrl.
+#include "ugv_advance.h"
+
+// functions for wifi ctrl.
+#include "wifi_ctrl.h"
+
+// functions for esp-now.
+#include "esp_now_ctrl.h"
+
+// functions for uart json ctrl.
+#include "uart_ctrl.h"
+
+// functions for http & web server.
+#include "http_server.h"
+
+
+void moduleType_RoArmM2() {
+  unsigned long curr_time = millis();
+  if (curr_time - prev_time >= 10){
+    constantHandle();
+    prev_time = curr_time;
+  }
+
+  RoArmM2_getPosByServoFeedback();
+  
+  // esp-now flow ctrl as a flow-leader.
+  switch(espNowMode) {
+  case 1: espNowGroupDevsFlowCtrl();break;
+  case 2: espNowSingleDevFlowCtrl();break;
+  }
+
+  if (InfoPrint == 2) {
+    RoArmM2_infoFeedback();
+  }
+}
+
+
+void moduleType_Gimbal() {
+  getGimbalFeedback();
+  gimbalSteady(steadyGoalY);
+}
+
+
+void setup() {
+  Serial.begin(115200);
+  Wire.begin(S_SDA, S_SCL);
+  while(!Serial) {}
+
+  ina219_init();
+  inaDataUpdate();
+
+  // set mainType & moduleType.
+  // mainType: 1.WAVE ROVER, 2.UGV02, 3.UGV01
+  // moduleType: 0.Null, 1.RoArm, 2.PT
+  mm_settings(mainType, moduleType);
+
+  init_oled();
+  if (mainType == 1) {
+    screenLine_0 = "WAVE ROVER";
+  } else if (mainType == 2) {
+    screenLine_0 = "UGV";
+  } else if (mainType == 3) {
+    screenLine_0 = "UGV";
+  } 
+  
+  screenLine_1 = "version: 1.00";
+  screenLine_2 = "starting...";
+  screenLine_3 = "";
+  oled_update();
+
+  delay(1200);
+
+  // functions for IMU.
+  imu_init();
+
+  // functions for the leds on ugv.
+  led_pin_init();
+
+  // init the littleFS funcs in files_ctrl.h
+  screenLine_2 = screenLine_3;
+  screenLine_3 = "Initialize LittleFS";
+  oled_update();
+  if(InfoPrint == 1){Serial.println("Initialize LittleFS for Flash files ctrl.");}
+  initFS();
+
+  // init the funcs in switch_module.h
+  screenLine_2 = screenLine_3;
+  screenLine_3 = "Initialize 12V-switch ctrl";
+  oled_update();
+  if(InfoPrint == 1){Serial.println("Initialize the pins used for 12V-switch ctrl.");}
+  movtionPinInit();
+
+  // servos power up
+  screenLine_2 = screenLine_3;
+  screenLine_3 = "Power up the servos";
+  oled_update();
+  if(InfoPrint == 1){Serial.println("Power up the servos.");}
+  delay(500);
+  
+  // init servo ctrl functions.
+  screenLine_2 = screenLine_3;
+  screenLine_3 = "ServoCtrl init UART2TTL...";
+  oled_update();
+  if(InfoPrint == 1){Serial.println("ServoCtrl init UART2TTL...");}
+  RoArmM2_servoInit();
+
+  // check the status of the servos.
+  screenLine_2 = screenLine_3;
+  screenLine_3 = "Bus servos status check...";
+  oled_update();
+  if(InfoPrint == 1){Serial.println("Bus servos status check...");}
+  RoArmM2_initCheck(false);
+
+  if(InfoPrint == 1 && RoArmM2_initCheckSucceed){
+    Serial.println("All bus servos status checked.");
+  }
+  if(RoArmM2_initCheckSucceed) {
+    screenLine_2 = "Bus servos: succeed";
+  } else {
+    screenLine_2 = "Bus servos: " + 
+    servoFeedback[BASE_SERVO_ID - 11].status +
+    servoFeedback[SHOULDER_DRIVING_SERVO_ID - 11].status +
+    servoFeedback[SHOULDER_DRIVEN_SERVO_ID - 11].status +
+    servoFeedback[ELBOW_SERVO_ID - 11].status +
+    servoFeedback[GRIPPER_SERVO_ID - 11].status;
+  }
+  screenLine_3 = ">>> Moving to init pos...";
+  oled_update();
+  RoArmM2_resetPID();
+  RoArmM2_moveInit();
+
+  screenLine_3 = "Reset joint torque to ST_TORQUE_MAX";
+  oled_update();
+  if(InfoPrint == 1){Serial.println("Reset joint torque to ST_TORQUE_MAX.");}
+  RoArmM2_dynamicAdaptation(0, ST_TORQUE_MAX, ST_TORQUE_MAX, ST_TORQUE_MAX, ST_TORQUE_MAX);
+
+  screenLine_3 = "WiFi init";
+  oled_update();
+  if(InfoPrint == 1){Serial.println("WiFi init.");}
+  initWifi();
+
+  screenLine_3 = "http & web init";
+  oled_update();
+  if(InfoPrint == 1){Serial.println("http & web init.");}
+  initHttpWebServer();
+
+  screenLine_3 = "ESP-NOW init";
+  oled_update();
+  if(InfoPrint == 1){Serial.println("ESP-NOW init.");}
+  initEspNow();
+
+  screenLine_3 = "UGV started";
+  oled_update();
+  if(InfoPrint == 1){Serial.println("UGV started.");}
+
+  getThisDevMacAddress();
+
+  updateOledWifiInfo();
+
+  initEncoders();
+
+  pidControllerInit();
+
+  screenLine_2 = String("MAC:") + macToString(thisDevMac);
+  oled_update();
+
+  led_pwm_ctrl(0, 0);
+
+  if(InfoPrint == 1){Serial.println("Application initialization settings.");}
+  createMission("boot", "these cmds run automatically at boot.");
+  missionPlay("boot", 1);
+}
+
+
+void loop() {
+  serialCtrl();
+  server.handleClient();
+
+  // read and compute the info of joints.
+  switch (moduleType) {
+  case 1: moduleType_RoArmM2();break;
+  case 2: moduleType_Gimbal();break;
+  }
+
+  // recv esp-now json cmd.
+  if(runNewJsonCmd) {
+    jsonCmdReceiveHandler();
+    jsonCmdReceive.clear();
+    runNewJsonCmd = false;
+  }
+
+  getLeftSpeed();
+
+  LeftPidControllerCompute();
+  
+  getRightSpeed();
+  
+  RightPidControllerCompute();
+  
+  oledInfoUpdate();
+
+  updateIMUData();
+
+  if (baseFeedbackFlow) {
+    baseInfoFeedback();
+  }
+
+  heartBeatCtrl();
+
+  size_t freeHeap = esp_get_free_heap_size();
+}

battery_ctrl.h

@@ -0,0 +1,28 @@
+#define INA219_ADDRESS 0x42
+INA219_WE ina219 = INA219_WE(INA219_ADDRESS);
+
+float shuntVoltage_mV = 0.0;
+float loadVoltage_V = 0.0;
+float busVoltage_V = 0.0;
+float current_mA = 0.0;
+float power_mW = 0.0; 
+bool ina219_overflow = false;
+
+void ina219_init(){
+  if(!ina219.init()){
+    Serial.println("INA219 not connected!");
+  }
+  ina219.setADCMode(BIT_MODE_9);
+  ina219.setPGain(PG_320);
+  ina219.setBusRange(BRNG_16);
+  ina219.setShuntSizeInOhms(0.01); // used in INA219.
+}
+
+void inaDataUpdate(){
+  shuntVoltage_mV = ina219.getShuntVoltage_mV();
+  busVoltage_V = ina219.getBusVoltage_V();
+  current_mA = ina219.getCurrent_mA();
+  power_mW = ina219.getBusPower();
+  loadVoltage_V  = busVoltage_V + (shuntVoltage_mV/1000);
+  ina219_overflow = ina219.getOverflow();
+}

data/devConfig.json

@@ -0,0 +1 @@
+{"wifi_mode_on_boot":3,"sta_ssid":"JSBZY-2.4G","sta_password":"waveshare0755","ap_ssid":"RoArm","ap_password":"12345678"}

data/wifiConfig.json

@@ -0,0 +1 @@
+{"wifi_mode_on_boot":3,"sta_ssid":"JSBZY-2.4G","sta_password":"waveshare0755","ap_ssid":"RoArm","ap_password":"12345678"}

esp_now_ctrl.h

@@ -0,0 +1,430 @@
+const int MAX_FOLLOWERS = 10;  // Maximum number of follower devices
+
+typedef struct struct_message {
+  byte devCode;
+  float base;
+  float shoulder;
+  float elbow;
+  float hand;
+  byte cmd;
+  char message[210];
+} struct_message;
+
+struct_message espNowMessage;
+struct_message espNowMegsRecv;
+esp_now_peer_info_t peerInfo;
+
+#define ESP_NOW_CHANNEL 0
+#define ESP_NOW_ENCRYPT false
+
+uint8_t thisDevMac[6];
+uint8_t singleFollowerDev[6];
+
+
+// input a mac[6]:{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF}
+// return String: "FF:FF:FF:FF:FF:FF"
+String macToString(uint8_t mac[6]) {
+  char macStr[18]; // 6 pairs of 2 characters + null terminator
+  snprintf(macStr, sizeof(macStr), "%02X:%02X:%02X:%02X:%02X:%02X", 
+           mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
+  return String(macStr);
+}
+
+
+void getThisDevMacAddress() {
+  WiFi.macAddress(thisDevMac);
+
+  thisMacStr = macToString(thisDevMac);
+
+  jsonInfoHttp.clear();
+  jsonInfoHttp["mac"] = thisMacStr;
+
+  Serial.println(thisMacStr);
+}
+
+
+void changeEspNowMode(byte inputMode) {
+  switch(inputMode) {
+  case 0: espNowMode = inputMode;
+    if (InfoPrint == 1) {Serial.println("esp-now mode: none");}
+    screenLine_3 = "ESP-NOW: NONE";
+    oled_update();
+
+    jsonInfoHttp.clear();
+    jsonInfoHttp["info"] = "esp-now mode: none";
+
+    break;
+  case 1: espNowMode = inputMode;
+    espNowMessage.cmd = 0;
+    if (InfoPrint == 1) {Serial.println("esp-now mode: flow-leader(group)");}
+    screenLine_3 = "ESP-NOW: F-LEADER-B";
+    oled_update();
+
+    jsonInfoHttp.clear();
+    jsonInfoHttp["info"] = "esp-now mode: flow-leader(group)";
+
+    break;
+  case 2: espNowMode = inputMode;
+    espNowMessage.cmd = 0;
+    if (InfoPrint == 1) {Serial.println("esp-now mode: flow-leader(single)");}
+    screenLine_3 = "ESP-NOW: F-LEADER-S";
+    oled_update();
+
+    jsonInfoHttp.clear();
+    jsonInfoHttp["info"] = "esp-now mode: flow-leader(single)";
+
+    break;
+  case 3: espNowMode = inputMode;
+    if (InfoPrint == 1) {Serial.println("esp-now mode: follower");}
+    screenLine_3 = "ESP-NOW: > FOLLOWER <";
+    oled_update();
+
+    jsonInfoHttp.clear();
+    jsonInfoHttp["info"] = "esp-now mode: follower";
+
+    break;
+  }
+}
+
+
+// callback when data is sent
+void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status) {
+  char macStr[18];
+  // Serial.print("Packet to: ");
+  // Copies the sender mac address to a string
+  snprintf(macStr, sizeof(macStr), "%02x:%02x:%02x:%02x:%02x:%02x",
+           mac_addr[0], mac_addr[1], mac_addr[2], mac_addr[3], mac_addr[4], mac_addr[5]);
+
+  jsonInfoHttp.clear();
+  jsonInfoHttp["T"] = CMD_ESP_NOW_SEND;
+  jsonInfoHttp["mac"] = macStr;
+  jsonInfoHttp["status"] = (status == ESP_NOW_SEND_SUCCESS ? 1 : 0);
+  jsonInfoHttp["megs"] = (status == ESP_NOW_SEND_SUCCESS ? "Delivery Success" : "Delivery Fail");
+
+  String getInfoJsonString;
+  serializeJson(jsonInfoHttp, getInfoJsonString);
+  Serial.println(getInfoJsonString);
+}
+
+
+void macStringToByteArray(const String& macString, uint8_t* byteArray) {
+  for (int i = 0; i < 6; i++) {
+    byteArray[i] = strtol(macString.substring(i * 3, i * 3 + 2).c_str(), NULL, 16);
+  }
+  return;
+}
+
+
+void OnDataRecv(const unsigned char* mac, const unsigned char* incomingData, int len) {
+  if (espNowMode != 3){
+    return;
+  }
+
+  memcpy(&espNowMegsRecv, incomingData, sizeof(espNowMegsRecv));
+  if (espNowMegsRecv.cmd == 3) {
+    char macStr[18];
+    snprintf(macStr, sizeof(macStr), "%02x:%02x:%02x:%02x:%02x:%02x",
+           mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
+
+    jsonInfoHttp.clear();
+    jsonInfoHttp["T"] = CMD_ESP_NOW_RECV;
+    jsonInfoHttp["mac"] = macStr;
+    jsonInfoHttp["megs"] = espNowMegsRecv.message;
+    
+    String getInfoJsonString;
+    serializeJson(jsonInfoHttp, getInfoJsonString);
+    Serial.println(getInfoJsonString);
+  }
+
+  if (!ctrlByBroadcast) {
+    if (memcmp(mac, mac_whitelist_broadcast, 6) != 0) {
+      return;
+    }
+  }
+
+  // if (InfoPrint == 1) {
+  //   Serial.print("Bytes received: ");Serial.println(len);
+  // }
+
+  switch(espNowMegsRecv.cmd) {
+    case 0: {
+      RoArmM2_allJointAbsCtrl(espNowMegsRecv.base,
+                              espNowMegsRecv.shoulder,
+                              espNowMegsRecv.elbow,
+                              espNowMegsRecv.hand,
+                              0,
+                              0);break;
+    }
+    case 1: {
+      DeserializationError err = deserializeJson(jsonCmdReceive, espNowMegsRecv.message);
+        if (err == DeserializationError::Ok) {
+          jsonCmdReceiveHandler();
+        };break;
+    }
+    case 2: {
+      DeserializationError err = deserializeJson(jsonCmdReceive, espNowMegsRecv.message);
+        if (err == DeserializationError::Ok) {
+          runNewJsonCmd = true;
+        };break;
+    }
+  }
+}
+
+
+void initEspNow() {
+  if (esp_now_init() != ESP_OK) {
+    // Serial.println("Error initializing ESP-NOW");
+    jsonInfoHttp.clear();
+    jsonInfoHttp["T"] = CMD_ESP_NOW_SEND;
+    jsonInfoHttp["status"] = 2;
+    jsonInfoHttp["megs"] = "Error initializing ESP-NOW";
+
+    String getInfoJsonString;
+    serializeJson(jsonInfoHttp, getInfoJsonString);
+    Serial.println(getInfoJsonString);
+    return;
+  }
+
+  // register esp-now sending call back function.
+  esp_now_register_send_cb(OnDataSent);
+
+  // register esp-now receving call back function.
+  esp_now_register_recv_cb(OnDataRecv);
+
+  // register peer
+  peerInfo.channel = ESP_NOW_CHANNEL;  
+  peerInfo.encrypt = ESP_NOW_ENCRYPT;
+}
+
+
+void registerNewFollowerToPeer(String inputMac) {
+  if (inputMac.length() != 17) {
+    // Serial.println("invalid MAC address format.");
+    jsonInfoHttp.clear();
+    jsonInfoHttp["T"] = CMD_ESP_NOW_SEND;
+    jsonInfoHttp["status"] = 3;
+    jsonInfoHttp["megs"] = "invalid MAC address format.";
+
+    String getInfoJsonString;
+    serializeJson(jsonInfoHttp, getInfoJsonString);
+    Serial.println(getInfoJsonString);
+    return;
+  }
+
+  uint8_t macArray[6];
+  macStringToByteArray(inputMac, macArray);
+  for (int i = 0; i < 6; i++) {
+    singleFollowerDev[i] = macArray[i];
+  }
+  memcpy(peerInfo.peer_addr, macArray, 6);
+  if (esp_now_add_peer(&peerInfo) != ESP_OK) {
+    // Serial.println("Failed to add peer: " + inputMac);
+    jsonInfoHttp.clear();
+    jsonInfoHttp["T"] = CMD_ESP_NOW_SEND;
+    jsonInfoHttp["status"] = 4;
+    jsonInfoHttp["megs"] = "Failed to add peer.";
+    jsonInfoHttp["mac"] = inputMac;
+
+    String getInfoJsonString;
+    serializeJson(jsonInfoHttp, getInfoJsonString);
+    Serial.println(getInfoJsonString);
+    return;
+  }
+  // Serial.println("add peer: " + inputMac);
+  jsonInfoHttp.clear();
+  jsonInfoHttp["T"] = CMD_ESP_NOW_SEND;
+  jsonInfoHttp["status"] = 5;
+  jsonInfoHttp["megs"] = "add peer.";
+  jsonInfoHttp["mac"] = inputMac;
+
+  String getInfoJsonString;
+  serializeJson(jsonInfoHttp, getInfoJsonString);
+  Serial.println(getInfoJsonString);
+}
+
+
+void deleteFollower(String inputMac) {
+  if (inputMac.length() != 17) {
+    // Serial.println("invalid MAC address format.");
+    jsonInfoHttp.clear();
+    jsonInfoHttp["T"] = CMD_ESP_NOW_SEND;
+    jsonInfoHttp["status"] = 3;
+    jsonInfoHttp["megs"] = "invalid MAC address format.";
+
+    String getInfoJsonString;
+    serializeJson(jsonInfoHttp, getInfoJsonString);
+    Serial.println(getInfoJsonString);
+    return;
+  }
+
+  uint8_t macArray[6];
+  macStringToByteArray(inputMac, macArray);
+  esp_now_del_peer(macArray);
+
+  jsonInfoHttp.clear();
+  jsonInfoHttp["T"] = CMD_ESP_NOW_SEND;
+  jsonInfoHttp["status"] = 6;
+  jsonInfoHttp["megs"] = "delete peer.";
+  jsonInfoHttp["mac"]  = inputMac;
+
+  String getInfoJsonString;
+  serializeJson(jsonInfoHttp, getInfoJsonString);
+  Serial.println(getInfoJsonString);
+
+  // if (InfoPrint == 1) {
+  //   Serial.println("delete peer: " + inputMac);
+  // }
+}
+
+
+void espNowGroupSend(byte devCodeIn, float bIn, float sIn, float eIn, float hIn, byte cmdIn, String messageIn) {
+  espNowMessage.devCode  = devCodeIn;
+  espNowMessage.base     = bIn;
+  espNowMessage.shoulder = sIn;
+  espNowMessage.elbow    = eIn;
+  espNowMessage.hand     = hIn;
+  espNowMessage.cmd      = cmdIn;
+  strcpy(espNowMessage.message, messageIn.c_str());
+
+  esp_err_t result = esp_now_send(0, (uint8_t *) &espNowMessage, sizeof(struct_message));
+  // if (result == ESP_OK) {
+  //   Serial.println("Sent with success");
+  // }
+  // else {
+  //   Serial.println("Error sending the data");
+  // }
+  jsonInfoHttp.clear();
+  jsonInfoHttp["T"] = CMD_ESP_NOW_SEND;
+  jsonInfoHttp["status"] = (result == ESP_OK ? 8 : 7);
+  jsonInfoHttp["megs"] = (result == ESP_OK ? "sent with success." : "error sending the data.");
+
+  String getInfoJsonString;
+  serializeJson(jsonInfoHttp, getInfoJsonString);
+  Serial.println(getInfoJsonString);
+}
+
+
+void espNowSingleDevSend(String inputMac, byte devCodeIn, float bIn, float sIn, float eIn, float hIn, byte cmdIn, String messageIn){
+  if (inputMac.length() != 17) {
+    // Serial.println("invalid MAC address format.");
+    jsonInfoHttp.clear();
+    jsonInfoHttp["T"] = CMD_ESP_NOW_SEND;
+    jsonInfoHttp["status"] = 3;
+    jsonInfoHttp["megs"] = "invalid MAC address format.";
+
+    String getInfoJsonString;
+    serializeJson(jsonInfoHttp, getInfoJsonString);
+    Serial.println(getInfoJsonString);
+    return;
+  }
+
+  espNowMessage.devCode  = devCodeIn;
+  espNowMessage.base     = bIn;
+  espNowMessage.shoulder = sIn;
+  espNowMessage.elbow    = eIn;
+  espNowMessage.hand     = hIn;
+  espNowMessage.cmd      = cmdIn;
+  // espNowMessage.message  = messageIn;
+  strcpy(espNowMessage.message, messageIn.c_str());
+
+  uint8_t macArray[6];
+  macStringToByteArray(inputMac, macArray);
+  for (int i = 0; i < 6; i++) {
+    singleFollowerDev[i] = macArray[i];
+  }
+  esp_err_t result = esp_now_send(
+    macArray, 
+    (uint8_t *) &espNowMessage,
+    sizeof(struct_message));
+
+  // if (result == ESP_OK) {
+  //   Serial.println("Sent with success");
+  // }
+  // else {
+  //   Serial.println("Error sending the data");
+  // }
+  jsonInfoHttp.clear();
+  jsonInfoHttp["T"] = CMD_ESP_NOW_SEND;
+  jsonInfoHttp["status"] = (result == ESP_OK ? 8 : 7);
+  jsonInfoHttp["megs"] = (result == ESP_OK ? "sent with success." : "error sending the data.");
+
+  String getInfoJsonString;
+  serializeJson(jsonInfoHttp, getInfoJsonString);
+  Serial.println(getInfoJsonString);
+}
+
+
+void espNowSingleDevFlowCtrl() {
+  espNowMessage.base     = radB;
+  espNowMessage.shoulder = radS;
+  espNowMessage.elbow    = radE;
+  espNowMessage.hand     = radG;
+
+  esp_err_t result = esp_now_send(singleFollowerDev, 
+                                  (uint8_t *) &espNowMessage,
+                                  sizeof(struct_message));
+
+  // if (result == ESP_OK) {
+  //   Serial.println("Sent with success");
+  // }
+  // else {
+  //   Serial.println("Error sending the data");
+  // }
+  jsonInfoHttp.clear();
+  jsonInfoHttp["T"] = CMD_ESP_NOW_SEND;
+  jsonInfoHttp["status"] = (result == ESP_OK ? 8 : 7);
+  jsonInfoHttp["megs"] = (result == ESP_OK ? "sent with success." : "error sending the data.");
+
+  String getInfoJsonString;
+  serializeJson(jsonInfoHttp, getInfoJsonString);
+  Serial.println(getInfoJsonString);
+}
+
+
+void espNowGroupDevsFlowCtrl() {
+  espNowMessage.base     = radB;
+  espNowMessage.shoulder = radS;
+  espNowMessage.elbow    = radE;
+  espNowMessage.hand     = radG;
+
+  esp_err_t result = esp_now_send(0, (uint8_t *) &espNowMessage, sizeof(struct_message));
+  // if (result == ESP_OK) {
+  //   Serial.println("Sent with success");
+  // }
+  // else {
+  //   Serial.println("Error sending the data");
+  // }
+  jsonInfoHttp.clear();
+  jsonInfoHttp["T"] = CMD_ESP_NOW_SEND;
+  jsonInfoHttp["status"] = (result == ESP_OK ? 8 : 7);
+  jsonInfoHttp["megs"] = (result == ESP_OK ? "sent with success." : "error sending the data.");
+
+  String getInfoJsonString;
+  serializeJson(jsonInfoHttp, getInfoJsonString);
+  Serial.println(getInfoJsonString);
+}
+
+
+void changeBroadcastMode(bool inputMode, String inputMac) {
+  ctrlByBroadcast = inputMode;
+
+  jsonInfoHttp.clear();
+  jsonInfoHttp["mode"]  = inputMode;
+
+  uint8_t macArray[6];
+  macStringToByteArray(inputMac, macArray);
+  for (int i = 0; i < 6; i++) {
+    mac_whitelist_broadcast[i] = macArray[i];
+  }
+
+  if (InfoPrint == 1) {
+    if (ctrlByBroadcast) {
+      Serial.println("it can be ctrl by esp-now broadcast cmd.");
+      jsonInfoHttp["info"] = "it can be ctrl by esp-now broadcast cmd.";
+      jsonInfoHttp["leader mac"] = inputMac;
+    } else {
+      Serial.println("it won't be ctrl by esp-now broadcast cmd.");
+      jsonInfoHttp["info"] = "it won't be ctrl by esp-now broadcast cmd, leader mac: "+inputMac;
+    }
+  }
+}

files_ctrl.h

@@ -0,0 +1,319 @@
+// funcs for editing the files in flash.
+
+bool flashStatus = false;
+
+// initialize littleFS for flash file system ctrl.
+void initFS() {
+	if (!LittleFS.begin(true)){
+		if (InfoPrint == 1) {Serial.println("LittleFS mount failed.");}
+		flashStatus = false;
+	}
+	else {
+		if (InfoPrint == 1) {Serial.println("LittleFS mount succeed.");}
+		flashStatus = true;
+	}
+}
+
+
+// get the free space of flash.
+uint32_t freeFlashSpace(){
+  size_t total = LittleFS.totalBytes();
+  size_t used = LittleFS.usedBytes();
+  uint32_t freeSpace = total - used;
+  if (InfoPrint == 1) {
+  	Serial.print("totalBytes:\t");Serial.print(total);
+  	Serial.println(" bytes");
+  	Serial.print("free flash memory:\t");Serial.print(freeSpace);
+		Serial.println(" bytes");
+  }
+
+  jsonInfoHttp.clear();
+  jsonInfoHttp["info"] = "free flash space";
+  jsonInfoHttp["total"] = total;
+  jsonInfoHttp["free"] = freeSpace;
+  
+  return freeSpace; 
+}
+
+
+// scan all the files saved in flash.
+void scanFlashContents() {
+		jsonInfoHttp.clear();
+    
+    File root = LittleFS.open("/");
+    if (!root.isDirectory()) {
+    	if (InfoPrint == 1) {
+    		Serial.println("error: not a directory.");
+    		jsonInfoHttp["info"] = "error: not a directory.";
+    		return;
+    	}
+    }
+
+    jsonInfoHttp["info"] = "reading files and the first line";
+    File file = root.openNextFile();
+    while (file) {
+        if (!file.isDirectory()) {
+        	Serial.println(">>>---=== File Name and First line ===---<<<");
+        	Serial.println("[file]: [" + String(file.name()) + "]");
+        	Serial.println("[first line]:");
+        	String line = file.readStringUntil('\n');
+
+		    		if (line) {
+		    			Serial.println(line);
+		    			jsonInfoHttp[file.name()] = line;
+		    		} else {
+		    			Serial.println("no content.");
+		    			jsonInfoHttp[file.name()] = "[null]";
+		    		}
+		        file.close();
+        } else if (file.isDirectory()) {
+        	if (file) {
+        		Serial.println("Failed to open file: " + String(file.name()));
+        		jsonInfoHttp[file.name()] = "[failed to open]";
+        	}
+        }
+        file = root.openNextFile();
+    }
+}
+
+
+// create a new file and input the content.
+bool createFile(String fileName, String fileContent) {
+	jsonInfoHttp.clear();
+	if (!flashStatus) {
+		if (InfoPrint == 1) {Serial.println("LittleFS mount failed.");}
+  	jsonInfoHttp["info"] = "LittleFS mount failed.";
+		return false;
+	}
+
+	if (LittleFS.exists("/"+fileName)) {
+		if (InfoPrint == 1) {Serial.println("file already exists.");}
+		jsonInfoHttp["info"] = "file already exists.";
+		return false;
+	}
+
+	File file = LittleFS.open("/"+fileName, "w");
+	if (file) {
+		// file.println("{\"name\":\"" + fileName + "\",\"intro\":\"" + fileContent + "\"}");
+		file.println(fileContent);
+		file.close();
+		if (InfoPrint == 1) {Serial.println("file created successfully.");}
+		jsonInfoHttp["info"] = "file created successfully.";
+		return true;
+	} else {
+		if (InfoPrint == 1) {Serial.println("file creation failed.");}
+		jsonInfoHttp["info"] = "file creation failed.";
+		return false;
+	}
+}
+
+
+// read a file, this function return the lineNum.
+int readFile(String fileName) {
+  jsonInfoHttp.clear();
+  jsonInfoHttp["info"] = "reading file";
+	File file = LittleFS.open("/" + fileName, "r");
+	if (!file) {
+		Serial.println("file not found.");
+		jsonInfoHttp["info"] = "file not found";
+		return -1;
+	}
+
+	Serial.println("---=== File Content ===---");
+	Serial.println("reading file: [" + fileName + "] starts:");
+	
+	jsonInfoHttp["name"] = fileName;
+
+	int _LineNum = -1;
+	while (file.available()) {
+		_LineNum++;
+		String line = file.readStringUntil('\n');
+		Serial.print("[lineNum: ");Serial.print(_LineNum+1);Serial.print(" ] - ");
+		Serial.println(line);
+
+		jsonInfoHttp["lineNum_"+String(_LineNum+1)] = line;
+	}
+
+	Serial.println("^^^ ^^^ ^^^ reading file: " + fileName + " ends. ^^^ ^^^ ^^^");
+	file.close();
+
+	return _LineNum + 1;
+}
+
+
+// delete a file.
+bool deleteFile(String inputName) {
+	jsonInfoHttp.clear();
+	if (!flashStatus) {
+		if (InfoPrint == 1) {Serial.println("LittleFS mount failed.");}
+		jsonInfoHttp["info"] = "LittleFS mount failed.";
+		return false;
+	}
+
+	if (!LittleFS.exists("/" + inputName)) {
+		if (InfoPrint == 1) {Serial.println("file already deleted.");}
+		jsonInfoHttp["info"] = "file already deleted.";
+		return false;
+	}
+
+	LittleFS.remove("/" + inputName);
+	if (InfoPrint == 1) {Serial.println("file deleted successfully.");}
+	jsonInfoHttp["info"] = "file deleted successfully.";
+	return true;
+}
+
+
+// add content at the end of a file.
+void appendLine(String fileName, String appendContent) {
+	Serial.println("--- --- --- RAW FILE -- --- ---");
+	if (readFile(fileName) == -1) {return;}
+
+	File file = LittleFS.open("/" + fileName, "a");
+
+	if(!file){
+	Serial.println("Error opening file for appending.");
+	return;
+	}
+
+	file.println(appendContent);
+	file.close();
+
+	Serial.println("--- --- --- NEW FILE -- --- ---");
+	jsonInfoHttp.clear();
+	readFile(fileName);
+}
+
+
+// insert a new line under the lineNum.
+void insertLine(String filename, int lineNum, String newLineString) {
+	Serial.println("--- --- --- RAW FILE -- --- ---");
+	int _LineNum = readFile(filename);
+	if (_LineNum == -1) {return;}
+
+	String lines[_LineNum+1];
+
+	File file = LittleFS.open("/" + filename, "r");
+	if(!file){
+		Serial.println("Error opening file for writing.");
+		return;
+	}
+
+	int i = 0;
+	while (file.available()) {
+		if (i == lineNum - 1) {
+			lines[i] = newLineString;
+			i++;
+		}
+		lines[i] = file.readStringUntil('\n');
+		i++;
+		}
+	file.close();
+
+	file = LittleFS.open("/" + filename, "w");
+	for(int j=0; j<i; j++){
+		file.println(lines[j]);
+	}
+	file.close();
+
+	Serial.println("--- --- --- NEW FILE -- --- ---");
+	jsonInfoHttp.clear();
+	readFile(filename);
+}
+
+
+// change a single line in the file.
+void replaceLine(String filename, int lineNum, String newLineString) {
+	Serial.println("--- --- --- RAW FILE -- --- ---");
+	int _LineNum = readFile(filename);
+	if (_LineNum == -1) {return;}
+
+	String lines[_LineNum]; 
+
+	File file = LittleFS.open("/" + filename, "r");
+	if(!file){
+		Serial.println("Error opening file.");
+		return;
+	}  
+
+	int i = 0;
+	while (file.available()) {
+		lines[i] = file.readStringUntil('\n');
+		i++;
+	}
+	file.close();
+
+	lines[lineNum-1] = newLineString;
+
+	file = LittleFS.open("/" + filename, "w");
+	for(int j=0; j<i; j++){
+		file.println(lines[j]);
+	}
+	file.close();
+
+	Serial.println("--- --- --- NEW FILE -- --- ---");
+	jsonInfoHttp.clear();
+	readFile(filename);
+}
+
+
+// read a single line from file.
+String readSingleLine(String filename, int lineNum) {
+	File file = LittleFS.open("/" + filename, "r");
+	if(!file){
+		Serial.println("Error opening file.");
+		return "";
+	}  
+	jsonInfoHttp.clear();
+	String line;
+	int i = 0;
+	while (file.available()) {
+		line = file.readStringUntil('\n');
+		if (i == lineNum-1) {
+			file.close();
+			if (InfoPrint == 1) {
+				Serial.println(line);
+				jsonInfoHttp["filename"] = filename;
+				jsonInfoHttp["lineNum"]  = lineNum;
+			}
+			return line;
+		}
+		i++;
+	}
+	file.close();
+	if (InfoPrint == 1) {
+		Serial.println("[line not found]");
+	}
+	return "";
+}
+
+
+void deleteSingleLine(String fileName, int lineNum){
+  Serial.println("--- --- --- RAW FILE -- --- ---");
+  File file = LittleFS.open("/" + fileName, "r+");
+  readFile(fileName);
+  if(!file){
+    Serial.println("Error opening file for reading");
+    return;
+  }
+
+  String contents = "";
+  int i = 1;
+
+  while(file.available()){
+    String line = file.readStringUntil('\n');
+    if(i != lineNum){
+      contents += line + "\n";  
+    }
+    i++;
+  }
+
+  file.close();
+
+  file = LittleFS.open("/" + fileName, "w");
+  file.print(contents);
+  file.close();
+
+  Serial.println("--- --- --- NEW FILE -- --- ---");
+  jsonInfoHttp.clear();
+  readFile(fileName);
+}

gimbal_module.h

@@ -0,0 +1,198 @@
+u8 gimbalID[2] = {GIMBAL_PAN_ID, GIMBAL_TILT_ID};
+
+s16 gimbalPos[2];
+u16 gimbalSpd[2];
+u8  gimbalAcc[2];
+
+ServoFeedback gimbalFeedback[2];
+// [0] PAN
+// [1] TILT
+
+float steadyGoalY = 0;
+
+float constrainFloat(float value, float min, float max) {
+  if (value < min) {
+    return min;
+  } else if (value > max) {
+    return max;
+  }
+  return value;
+}
+
+float mapFloat(float value, float fromLow, float fromHigh, float toLow, float toHigh) {
+  return (value - fromLow) * (toHigh - toLow) / (fromHigh - fromLow) + toLow;
+}
+
+void gimbalCtrlSimple(float Xinput, float Yinput, float spdInput, float accInput) {
+  Xinput = constrainFloat(Xinput, -180, 180);
+  Yinput = constrainFloat(Yinput, -30, 90);
+
+  gimbalPos[0] = 2047 + (int)round(map(Xinput, 0, 360, 0, 4095));
+  gimbalPos[1] = 2047 - (int)round(map(Yinput, 0, 360, 0, 4095));
+
+  gimbalSpd[0] = (int)round(map(spdInput, 0, 360, 0, 4095));
+  gimbalSpd[1] = (int)round(map(spdInput, 0, 360, 0, 4095));
+
+  gimbalAcc[0] = (int)round(map(accInput, 0, 360, 0, 4095));
+  gimbalAcc[1] = (int)round(map(accInput, 0, 360, 0, 4095));
+
+  st.SyncWritePosEx(gimbalID, 2, gimbalPos, gimbalSpd, gimbalAcc);
+}
+
+void gimbalCtrlMove(float Xinput, float Yinput, float spdInputX, float spdInputY) {
+  Xinput = constrainFloat(Xinput, -180, 180);
+  Yinput = constrainFloat(Yinput, -30, 90);
+
+  spdInputX = constrain(spdInputX, 1, 2500);
+  spdInputY = constrain(spdInputY, 1, 2500);
+
+  gimbalPos[0] = 2047 + (int)round(map(Xinput, 0, 360, 0, 4095));
+  gimbalPos[1] = 2047 - (int)round(map(Yinput, 0, 360, 0, 4095));
+
+  gimbalSpd[0] = spdInputX;
+  gimbalSpd[1] = spdInputY;
+
+  gimbalAcc[0] = 0;
+  gimbalAcc[1] = 0;
+
+  st.SyncWritePosEx(gimbalID, 2, gimbalPos, gimbalSpd, gimbalAcc);
+}
+
+
+//mapFloat(float value, float fromLow, float fromHigh, float toLow, float toHigh)
+float panAngleCompute(int inputPos) {
+  return mapFloat((inputPos - 2047), 0, 4095, 0, 360);
+}
+
+float tiltAngleCompute(int inputPos) {
+  return mapFloat((2047 - inputPos), 0, 4095, 0, 360);
+}
+
+void gimbalCtrlStop() {
+  st.EnableTorque(GIMBAL_PAN_ID, 0);
+  st.EnableTorque(GIMBAL_TILT_ID, 0);
+  delay(SERVO_STOP_DELAY);
+  st.EnableTorque(GIMBAL_PAN_ID, 1);
+  st.EnableTorque(GIMBAL_TILT_ID, 1);
+}
+
+void getGimbalFeedback() {
+  if(st.FeedBack(GIMBAL_PAN_ID)!=-1) {
+    gimbalFeedback[0].status = true;
+    gimbalFeedback[0].pos = st.ReadPos(-1);
+    gimbalFeedback[0].speed = st.ReadSpeed(-1);
+    gimbalFeedback[0].load = st.ReadLoad(-1);
+    gimbalFeedback[0].voltage = st.ReadVoltage(-1);
+    gimbalFeedback[0].current = st.ReadCurrent(-1);
+    gimbalFeedback[0].temper = st.ReadTemper(-1);
+    gimbalFeedback[0].mode = st.ReadMode(GIMBAL_PAN_ID);
+  } else{
+    servoFeedback[0].status = false;
+    if(InfoPrint == 1){
+      jsonInfoHttp.clear();
+      jsonInfoHttp["T"] = 1005;
+      jsonInfoHttp["id"] = GIMBAL_PAN_ID;
+      jsonInfoHttp["status"] = 0;
+      String getInfoJsonString;
+      serializeJson(jsonInfoHttp, getInfoJsonString);
+      Serial.println(getInfoJsonString);
+    }
+  }
+
+  if(st.FeedBack(GIMBAL_TILT_ID)!=-1) {
+    gimbalFeedback[1].status = true;
+    gimbalFeedback[1].pos = st.ReadPos(-1);
+    gimbalFeedback[1].speed = st.ReadSpeed(-1);
+    gimbalFeedback[1].load = st.ReadLoad(-1);
+    gimbalFeedback[1].voltage = st.ReadVoltage(-1);
+    gimbalFeedback[1].current = st.ReadCurrent(-1);
+    gimbalFeedback[1].temper = st.ReadTemper(-1);
+    gimbalFeedback[1].mode = st.ReadMode(GIMBAL_TILT_ID);
+  } else{
+    servoFeedback[1].status = false;
+    if(InfoPrint == 1){
+      jsonInfoHttp.clear();
+      jsonInfoHttp["T"] = 1005;
+      jsonInfoHttp["id"] = GIMBAL_TILT_ID;
+      jsonInfoHttp["status"] = 0;
+      String getInfoJsonString;
+      serializeJson(jsonInfoHttp, getInfoJsonString);
+      Serial.println(getInfoJsonString);
+    }
+  }
+}
+
+void gimbalSteadySet(bool inputCmd, float inputY) {
+  steadyMode = inputCmd;
+  if (inputY < -45) {
+    inputY = -45;
+  } else if (inputY > 90) {
+    inputY = 90;
+  }
+  steadyGoalY = inputY;
+}
+
+
+void gimbalSteady(float inputBiasY) {
+  if (!steadyMode) {
+    return;
+  }
+  gimbalCtrlSimple(0, inputBiasY - icm_pitch, 0, 0);
+}
+
+
+void gimbalUserCtrl(int inputX, int inputY, int inputSpd) {
+  static float goalX = 0;
+  static float goalY = 0;
+
+  if(inputX == -1 && inputY == 1){
+    goalX = -180;
+    goalY = 90;
+  }
+  else if(inputX == 0 && inputY == 1){
+    goalY = 90;
+  }
+  else if(inputX == 1 && inputY == 1){
+    goalX = 180;
+    goalY = 90;
+  }
+  else if(inputX == -1 && inputY == 0){
+    goalX = -180;
+  }
+  else if(inputX == 1 && inputY == 0){
+    goalX = 180;
+  }
+  else if(inputX == -1 && inputY == -1){
+    goalX = -180;
+    goalY = -45;
+  }
+  else if(inputX == 0 && inputY == -1){
+    goalY = -45;
+  }
+  else if(inputX == 1 && inputY == -1){
+    goalX = 180;
+    goalY = -45;
+  }
+
+  if(inputX == 2 && inputY == 2){
+    gimbalCtrlSimple(0, 0, 0, 10);
+  }
+  else{
+    gimbalCtrlSimple(goalX, goalY, inputSpd, 0);
+    if(inputX == 0){
+      servoTorqueCtrl(GIMBAL_PAN_ID, 0);
+      delay(5);
+      servoTorqueCtrl(GIMBAL_PAN_ID, 1);
+      getGimbalFeedback();
+      goalX = panAngleCompute(gimbalFeedback[0].pos);
+    }
+    if(inputY == 0){
+      servoTorqueCtrl(GIMBAL_TILT_ID, 0);
+      delay(5);
+      servoTorqueCtrl(GIMBAL_TILT_ID, 1);
+      getGimbalFeedback();
+      goalY = tiltAngleCompute(gimbalFeedback[1].pos);
+    }
+  }
+
+}

http_server.h

@@ -0,0 +1,31 @@
+#include "web_page.h"
+
+// Create AsyncWebServer object on port 80
+WebServer server(80);
+
+void handleRoot(){
+  server.send(200, "text/html", index_html); //Send web page
+}
+
+void webCtrlServer(){
+  server.on("/", handleRoot);
+
+  server.on("/js", [](){
+    String jsonCmdWebString = server.arg(0);
+    deserializeJson(jsonCmdReceive, jsonCmdWebString);
+    jsonCmdReceiveHandler();
+    serializeJson(jsonInfoHttp, jsonFeedbackWeb);
+    server.send(200, "text/plane", jsonFeedbackWeb);
+    jsonFeedbackWeb = "";
+    jsonInfoHttp.clear();
+    jsonCmdReceive.clear();
+  });
+
+  // Start server
+  server.begin();
+  Serial.println("Server Starts.");
+}
+
+void initHttpWebServer(){
+  webCtrlServer();
+}

json_cmd.h

@@ -0,0 +1,575 @@
+#define FEEDBACK_BASE_INFO  1001
+#define FEEDBACK_IMU_DATA   1002
+// esp-now recv
+// {"T":1003,"mac":"FF:FF:FF:FF:FF:FF","megs":"hello!"}
+#define CMD_ESP_NOW_RECV 1003
+// esp-now send status
+// 0:failed   1:succeed   2:Error initializing ESP-NOW
+// 3:invalid MAC address format.
+// 4:Failed to add peer.
+// 5:add peer.   6:delete peer.
+// 7:error sending the data.   8:sent with success.
+// {"T":1004,"mac":"FF:FF:FF:FF:FF:FF","status":1,"megs":"xxx"}
+#define CMD_ESP_NOW_SEND 1004
+// bus servos error feedback
+// {"T":1005,"id":1,"status":1}
+#define CMD_BUS_SERVO_ERROR 1005
+
+
+
+// ---===< EoAT type settings. >===---
+
+// modeType 0: gripper
+// {"T":124,"mode":0}
+// modeType 1: wrist
+// {"T":124,"mode":1}
+#define CMD_EOAT_TYPE 124
+
+// EoAT assemble.
+// mount position: 0 - edge
+//                 1 - D-3.2
+//                 2 - D-4.2
+//                 3 - D-10.2
+//                   -------L3A-----------O==L2B===
+//                   |                    ^       ||
+//                  L3B                   |       ||
+//                   |              ELBOW_JOINT   ||
+//     pos->X--L4A---O                           L2A
+//          |                                     ||
+//     |   L4B                                    ||
+//   /      |                                     ||
+//  PI X-EA-X                   SHOULDER_JOINT -> OO
+//   \ |                                         [||]
+//    EB                                          L1
+//     |                                         [||]
+//    --------                      BASE_JOINT -> XX
+// unit:mm
+// {"T":125,"pos":3,"ea":0,"eb":20}
+#define CMD_CONFIG_EOAT	125
+
+
+
+// ---===< UGV ctrl. >===---
+// SPEED_INPUT
+// {"T":1,"L":0.5,"R":0.5}
+#define CMD_SPEED_CTRL	1
+
+// {"T":11,"L":164,"R":164} (input PWM +-255)
+#define CMD_PWM_INPUT	11
+
+// {"T":13,"X":0.1,"Z":0.3} (m/s,rad/s)(Not for the products without encoders)
+#define CMD_ROS_CTRL	13
+
+// MOTOR PID & WINDUP LIMITS
+// {"T":2,"P":200,"I":2500,"D":0,"L":255}
+// {"T":2,"P":20,"I":2500,"D":0,"L":255}
+// {"T":222,"name":"mission_a","step":"{\"T\":104,\"x\":235,\"y\":0,\"z\":234,\"t\":3.14,\"spd\":0.25}"}
+// {"T":222,"name":"boot","step":"{\"T\":2,\"P\":20,\"I\":2500,\"D\":0,\"L\":255}"}
+#define CMD_SET_MOTOR_PID	2
+
+// OLED INFO SET
+// {"T":3,"lineNum":0,"Text":"putYourTextHere"}
+#define CMD_OLED_CTRL	3
+
+// OLED DEFAULT
+// {"T":-3}
+#define CMD_OLED_DEFAULT	-3
+
+// MODULE TYPE
+// 0: nothing
+// 1: RoArm-M2-S
+// 2: Gimbal
+// {"T":4,"cmd":0}
+#define CMD_MODULE_TYPE	4
+
+
+// {"T":126}
+#define CMD_GET_IMU_DATA	126
+
+// the robot need to be put on a ground and kept still
+// getting the imu offset and set as default
+// this gonna take a while (5s)
+// {"T":127}
+#define CMD_CALI_IMU_STEP	127
+
+// {"T":128}
+#define CMD_GET_IMU_OFFSET	128
+
+// {"T":129,"x":-12,"y":0,"z":0}
+#define CMD_SET_IMU_OFFSET	129
+
+// {"T":130}
+#define CMD_BASE_FEEDBACK 	130
+
+// off: {"T":131,"cmd":0} [default]
+// 	on: {"T":131,"cmd":1}
+#define CMD_BASE_FEEDBACK_FLOW   131
+
+// set the extra delay time(ms) for feedback info
+// {"T":142,"cmd":0}
+#define CMD_FEEDBACK_FLOW_INTERVAL	142	// dev
+
+// set the echo mode of recving new cmd.
+// 0: [default]off
+// 1: on
+// {"T":143,"cmd":0}
+#define CMD_UART_ECHO_MODE	143
+
+
+
+// LIGHT/GIMBAL/MOVTION CTRL
+// {"T":132,"IO4":255,"IO5":255}
+#define CMD_LED_CTRL	132
+
+// GIMBAL CTRL(SIMPLE)
+// {"T":133,"X":45,"Y":45,"SPD":0,"ACC":0}
+#define CMD_GIMBAL_CTRL_SIMPLE	133
+
+// GIMBAL CTRL MOVE
+// {"T":134,"X":45,"Y":45,"SX":300,"SY":300}
+#define CMD_GIMBAL_CTRL_MOVE	134
+
+// GIMBAL CTRL STOP
+// {"T":135}
+#define CMD_GIMBAL_CTRL_STOP	135
+
+// CHANGE HEART BEAT DELAY
+// {"T":136,"cmd":3000}
+#define CMD_HEART_BEAT_SET	136
+
+// GIMBAL STEADY
+// off: {"T":137,"s":0,"y":0}
+//  on:	{"T":137,"s":1,"y":0}
+#define CMD_GIMBAL_STEADY	137
+
+// SET SPEED RATE
+// {"T":138,"L":1,"R":1}
+#define CMD_SET_SPD_RATE	138
+
+// GET SPEED RATE
+// {"T":139}
+#define CMD_GET_SPD_RATE	139
+
+// SAVE SPEED RATE
+// {"T":140}
+#define CMD_SAVE_SPD_RATE	140
+
+// GIMBAL USER CTRL
+// {"T":141,"X":0,"Y":0,"SPD":300}
+// -1: decrease
+//  1: increase
+//  0: stop
+//  2,2: middle
+#define CMD_GIMBAL_USER_CTRL	141
+
+
+// ---===< Arm ctrl. >===---
+
+// it moves to goal position directly.
+// without interpolation.
+// {"T":100}
+#define CMD_MOVE_INIT 100
+
+// {"T":101,"joint":0,"rad":0,"spd":0,"acc":10}
+// joint: 1-BASE_JOINT + ->left
+//        2-SHOULDER_JOINT + ->down
+//        3-ELBOW_JOINT + ->down
+//        4-EOAT_JOINT + ->grab/down
+// spd: steps/s
+// acc: steps/s^2
+#define CMD_SINGLE_JOINT_CTRL 101
+
+// {"T":102,"base":0,"shoulder":0,"elbow":1.57,"hand":1.57,"spd":0,"acc":10}
+// input the angle in rad(180°=3.1415926).
+#define CMD_JOINTS_RAD_CTRL 102
+
+// {"T":103,"axis":2,"pos":0,"spd":0.25}
+// axis: 1-x: 235.11
+//       2-y: 0
+//       3-z: 234.79
+//       4-t: 1.57
+#define CMD_SINGLE_AXIS_CTRL 103
+
+// // // // // // // // // // // // // // // // // // // // //
+// {"T":104,"x":235,"y":0,"z":234,"t":3.14,"spd":0.25}      //
+#define CMD_XYZT_GOAL_CTRL 104                              //
+// // // // // // // // // // // // // // // // // // // // //
+
+// {"T":1041,"x":235,"y":0,"z":234,"t":3.14}
+#define CMD_XYZT_DIRECT_CTRL 1041
+
+
+// {"T":105}
+// x: real x position.
+// y: real y position.
+// z: real z position.
+// t: real grab/hand angle in rad.
+// torB: base joint torque.
+// torS: shoulder joint torque.
+// torE: elbow joint torque.
+#define CMD_SERVO_RAD_FEEDBACK 105
+
+// release:
+// {"T":106,"cmd":1.57,"spd":0,"acc":0}
+// grab:
+// {"T":106,"cmd":3.14,"spd":0,"acc":0}
+// hand joint ctrl using angle in radius.
+// {"T":106,"cmd":4.0,"spd":0,"acc":0}
+#define CMD_EOAT_HAND_CTRL 106
+
+// {"T":107,"tor":200}
+#define CMD_EOAT_GRAB_TORQUE 107
+
+// {"T":108,"joint":3,"p":16,"i":0}
+// change the P&I of a joint.
+// BASE_JOINT     - 1
+// SHOULDER_JOINT - 2
+// ELBOW_JOINT    - 3
+// EOAT_JOINT     - 4
+// default p:32[servo] 16[RoArm-M2]
+//         i: 0[servo]  8[RoArm-M2 PID MODE ON]
+//         d: not used by default
+#define CMD_SET_JOINT_PID 108
+
+// {"T":109}
+// reset the PID.
+#define CMD_RESET_PID 109
+
+// set a new x-axis.
+// {"T":110,"xAxisAngle":0}
+#define CMD_SET_NEW_X 110
+
+// set delay time
+// {"T":111,"cmd":3000}
+#define CMD_DELAY_MILLIS 111
+
+// dynamic external force adaptation.
+// mode: 0 - stop: reset every limit torque to 1000.
+//       1 - start: set the joint limit torque. 
+// b, s, e, h = bassJoint, shoulderJoint, elbowJoint, handJoint
+// example:
+// starts. input the limit torque of every joint.
+// {"T":112,"mode":1,"b":60,"s":110,"e":50,"h":50}
+// stop
+// {"T":112,"mode":0,"b":1000,"s":1000,"e":1000,"h":1000}
+#define CMD_DYNAMIC_ADAPTATION 112
+
+// switch-12V ctrl.(NOT FOR UVG)
+// pwm: -255 ~ 0(off) ~ +255
+// {"T":113,"pwm_a":-255,"pwm_b":-255}
+#define CMD_SWITCH_CTRL 113
+
+// light ctrl.(NOT FOR UVG)
+// led: 0(off) - 255(max)
+// {"T":114,"led":255}
+#define CMD_LIGHT_CTRL 114
+
+// switch off.
+// {"T":115}
+#define CMD_SWITCH_OFF 115
+
+// ctrl a single joint abs angle in deg.
+// joint: 1-BASE_JOINT + ->left
+//        2-SHOULDER_JOINT + ->down
+//        3-ELBOW_JOINT + ->down
+//        4-EOAT_JOINT + ->grab/down
+// spd: speed, angle/s^2
+// acc: speed, angle/s^2 (max: 22.5)
+// {"T":121,"joint":1,"angle":0,"spd":10,"acc":10}
+#define CMD_SINGLE_JOINT_ANGLE 121
+
+// ctrl all joints
+// b - BASE
+// s - SHOULDER
+// e - ELBOW
+// h - HAND
+// spd - angle/s
+// acc - angle/s^2 (max: 22.5)
+// {"T":122,"b":0,"s":0,"e":90,"h":180,"spd":10,"acc":10}
+#define CMD_JOINTS_ANGLE_CTRL  122
+
+// constant ctrl
+// m: 0 - angle
+//    1 - xyzt
+// cmd: 0 - stop
+// 		1 - increase
+// 		2 - decrease
+// {"T":123,"m":0,"axis":0,"cmd":0,"spd":3}
+#define CMD_CONSTANT_CTRL  123
+
+// 124/125...131
+
+
+// === === === MISSION CTRL & FILE CTRL === === ===
+
+// scan files in flash.
+// {"T":200}
+#define CMD_SCAN_FILES 200
+
+// create a new file and input the content.
+// {"T":201,"name":"file.txt","content":"inputContentHere."}
+#define CMD_CREATE_FILE 201
+
+// get a file content.
+// {"T":202,"name":"file.txt"}
+#define CMD_READ_FILE 202
+
+// remove a file in flash.
+// {"T":203,"name":"file.txt"}
+#define CMD_DELETE_FILE 203
+
+// add a line at the end of a file.
+// {"T":204,"name":"file.txt","content":"inputContentHere."}
+#define CMD_APPEND_LINE 204
+
+// insert a new line as lineNum.
+// {"T":205,"name":"file.txt","lineNum":3,"content":"content"}
+#define CMD_INSERT_LINE 205
+
+// change a single line in the file.
+// {"T":206,"name":"file.txt","lineNum":3,"content":"Content"}
+#define CMD_REPLACE_LINE 206
+
+// read a single line from file.
+// {"T":207,"name":"file.txt","lineNum":3}
+#define CMD_READ_LINE 207
+
+// delete a single line from file.
+// {"T":208,"name":"file.txt","lineNum":3}
+#define CMD_DELETE_LINE 208
+
+
+// torque-lock ctrl.
+// off: {"T":210,"cmd":0}
+//  on: {"T":210,"cmd":1}
+#define CMD_TORQUE_CTRL 210
+
+
+
+// === === === mission & steps edit. === === ===
+
+// create a mission in flash: 
+// {"T":220,"name":"mission_a","intro":"test mission created in flash."}
+#define CMD_CREATE_MISSION 220
+
+// input the mission name and get the total content.
+// {"T":221,"name":"mission_a"}
+#define CMD_MISSION_CONTENT  221
+
+// {"T":144,"E":100,"Z":0,"R":0}
+#define CMD_ARM_CTRL_UI 144
+
+
+// append a new step at the end of the mission, using the step input.
+// {"T":222,"name":"mission_a","step":"{\"T\":104,\"x\":235,\"y\":0,\"z\":234,\"t\":3.14,\"spd\":0.25}"}
+#define CMD_APPEND_STEP_JSON 222
+
+// append a new step at the end of the mission, using the feedback.
+// {"T":223,"name":"mission_a","spd":0.25}
+#define CMD_APPEND_STEP_FB 223
+
+// append a new delay(ms) at the end of the mission.
+// {"T":224,"name":"mission_a","delay":3000}
+#define CMD_APPEND_DELAY 224
+
+
+
+// insert a new step as the stepNum
+// using the json string input.
+// {"T":225,"name":"mission_a","stepNum":3,"step":"{\"T\":104,\"x\":235,\"y\":0,\"z\":234,\"t\":3.14,\"spd\":0.25}"}
+// {"T":225,"name":"mission_a","stepNum":3,"step":"{\"T\":114,\"led\":255}"}
+#define CMD_INSERT_STEP_JSON 225
+
+// insert a new step as the stepNum
+// using the feedback.
+// {"T":226,"name":"mission_a","stepNum":3,"spd":0.25}
+#define CMD_INSERT_STEP_FB 226
+
+// insert a new delay(ms) at the stepNum.
+// {"T":227,"stepNum":3,"delay":3000}
+#define CMD_INSERT_DELAY 227
+
+
+
+// replace the cmd at stepNum
+// using json cmd input.
+// {"T":228,"name":"mission_a","stepNum":3,"step":"{\"T\":114,\"led\":255}"}
+#define CMD_REPLACE_STEP_JSON 228
+
+// replace the cmd at stepNum
+// using feedback.
+// {"T":229,"name":"mission_a","stepNum":3,"spd":0.25}
+#define CMD_REPLACE_STEP_FB 229
+
+// replace the cmd at stepNum with delay cmd.
+// {"T":230,"name":"mission_a","stepNum":3,"delay":3000}
+#define CMD_REPLACE_DELAY 230
+
+
+// delete a step
+// {"T":231,"name":"mission_a","stepNum":3}
+#define CMD_DELETE_STEP 231
+
+
+// input the mission name and a stepNum, it will move to the step.
+// {"T":241,"name":"mission_a","stepNum":3}
+#define CMD_MOVE_TO_STEP 241
+
+// input the mission name and repeatTimes to play a mission.
+// if repeatTimes = -1, it will loop forever.
+// {"T":242,"name":"mission_a","times":3}
+#define CMD_MISSION_PLAY 242
+
+
+
+// === === === ESP-NOW settings. === === ===
+
+// note: wifi must be running under STA(AP+STA) mode.
+// it will be controled by broadcast mac address.
+// {"T":300,"mode":1} [default]
+// it won't be controled by broadcast mac address, and add one mac to whitelist.
+// if there is no leader you can just fill 00:00:00:00:00:00 in it.
+// {"T":300,"mode":0,"mac":"CC:DB:A7:5B:E4:1C"}
+#define CMD_BROADCAST_FOLLOWER 300
+
+// set the mode of esp-now
+// espNowMode: 0 - none
+//             1 - flow-leader(group): sending cmds
+//             2 - flow-leader(single): sending cmds to a single follower
+//             3 - [default]follower: recv cmds
+// flow-leader - use cmd=0, ctrl servos in real time.
+//               leader uses the servos feedback pos to ctrl followers.
+// {"T":301,"mode":3}
+#define CMD_ESP_NOW_CONFIG    301
+
+// get this dev mac address.
+// {"T":302}
+#define CMD_GET_MAC_ADDRESS   302
+
+// add a new follower mac address to peer.
+// {"T":303,"mac":"FF:FF:FF:FF:FF:FF"}
+// {"T":303,"mac":"CC:DB:A7:5B:E4:1C"}
+// {"T":303,"mac":"CC:DB:A7:5C:1C:40"}
+// {"T":303,"mac":"CC:DB:A7:5C:E5:FC"}
+#define CMD_ESP_NOW_ADD_FOLLOWER  303
+
+// remove a follower from peer.
+// {"T":304,"mac":"FF:FF:FF:FF:FF:FF"}
+// {"T":304,"mac":"CC:DB:A7:5B:E4:1C"}
+// {"T":304,"mac":"CC:DB:A7:5C:1C:40"}
+// {"T":304,"mac":"CC:DB:A7:5C:E5:FC"}
+#define CMD_ESP_NOW_REMOVE_FOLLOWER 304
+
+// send info to more than one peer devs.
+// "FF:FF:FF:FF:FF:FF" can't be in the broadcast peer.
+// {"T":305,"dev":0,"b":0,"s":0,"e":1.57,"h":1.57,"cmd":0,"megs":"hello!"}
+#define CMD_ESP_NOW_GROUP_CTRL 305
+
+// send info to a single dev, or to every devs by using "FF:FF:FF:FF:FF:FF"
+// broadcast ctrl:
+// {"T":306,"mac":"FF:FF:FF:FF:FF:FF","dev":0,"b":0,"s":0,"e":1.57,"h":1.57,"cmd":0,"megs":"hello!"}
+// {"T":306,"mac":"FF:FF:FF:FF:FF:FF","dev":0,"b":0,"s":0,"e":0,"h":0,"cmd":1,"megs":"{\"T\":114,\"led\":255}"}
+// single ctrl:
+// {"T":306,"mac":"CC:DB:A7:5C:E5:FC","dev":0,"b":0,"s":0,"e":1.57,"h":1.57,"cmd":0,"megs":"hello!"}
+#define CMD_ESP_NOW_SINGLE 306
+
+
+
+// === === === wifi settings. === === ===
+
+// config the wifi mode on boot.
+// 0 - off
+// 1 - ap
+// 2 - sta
+// 3 - ap+sta
+// {"T":401,"cmd":3}
+#define CMD_WIFI_ON_BOOT 401
+
+// config ap mode.
+// {"T":402,"ssid":"UGV","password":"12345678"}
+#define CMD_SET_AP  402
+
+// config sta mode.
+// {"T":403,"ssid":"na","password":"ps"}
+#define CMD_SET_STA 403
+
+// config ap/sta mode.
+// {"T":404,"ap_ssid":"UGV","ap_password":"12345678","sta_ssid":"na","sta_password":"ps"}
+#define CMD_WIFI_APSTA   404
+
+// get wifi info.
+// {"T":405}
+#define CMD_WIFI_INFO    405
+
+// create a wifiConfig.json file
+// from the args already be using.
+// {"T":406}
+#define CMD_WIFI_CONFIG_CREATE_BY_STATUS 406
+
+// create a wifiConfig.json file
+// from the args input.
+// {"T":407,"mode":3,"ap_ssid":"UGV","ap_password":"12345678","sta_ssid":"na","sta_password":"ps"}
+#define CMD_WIFI_CONFIG_CREATE_BY_INPUT 407
+
+// disconnect wifi.
+// {"T":408}
+#define CMD_WIFI_STOP 408
+
+
+
+// === === === servo settings. === === ===
+
+// change a servo's ID.
+// {"T":501,"raw":1,"new":11}
+#define CMD_SET_SERVO_ID 501
+
+// set the current position as the middle position.
+// > BASE_SERVO_ID    11
+// > SHOULDER_DRIVING_SERVO_ID 12
+// > SHOULDER_DRIVEN_SERVO_ID  13
+// > ELBOW_SERVO_ID   14
+// > GRIPPER_SERVO_ID 15
+// {"T":502,"id":11}
+#define CMD_SET_MIDDLE   502
+
+// set the P/PID of a single servo.
+// {"T":503,"id":14,"p":16}
+#define CMD_SET_SERVO_PID   503
+
+
+
+// === === === esp32 settings. === === ===
+
+// esp-32 ctrl.
+// reboot device.
+// {"T":600}
+#define CMD_REBOOT 	600
+
+// get the size of free flash space
+// {"T":601}
+#define CMD_FREE_FLASH_SPACE	601
+
+// boot mission info.
+// {"T":602}
+#define CMD_BOOT_MISSION_INFO	602
+
+// reset boot mission.
+// {"T":603}
+#define CMD_RESET_BOOT_MISSION	603
+
+// if there is something wrong with wifi funcs, clear the nvs.
+// {"T":604}
+#define CMD_NVS_CLEAR	604
+
+// 2: flow feedback.
+// 1: [default]print debug info in serial.
+// 0: don't print debug info in serial.
+// {"T":605,"cmd":1}
+#define CMD_INFO_PRINT	605
+
+
+
+// === === === mainType & moduleType settings. === === ===
+// {"T":900,"main":1,"module":0}
+// main_type: 1-WAVE ROVER, 2-UGV02, 3-UGV01
+#define CMD_MM_TYPE_SET 900

movtion_module.h

@@ -0,0 +1,449 @@
+// switch parts
+int switch_pwm_A = 0;
+int switch_pwm_B = 0;
+bool usePIDCompute = false;
+float spd_rate_A = 1.0;
+float spd_rate_B = 1.0;
+bool heartbeatStopFlag = false;
+
+void movtionPinInit(){
+  pinMode(AIN1, OUTPUT);
+  pinMode(AIN2, OUTPUT);
+  pinMode(PWMA, OUTPUT);
+  pinMode(BIN1, OUTPUT);
+  pinMode(BIN2, OUTPUT);
+  pinMode(PWMB, OUTPUT);
+
+  ledcSetup(channel_A, freq, ANALOG_WRITE_BITS);
+  ledcAttachPin(PWMA, channel_A);
+
+  ledcSetup(channel_B, freq, ANALOG_WRITE_BITS);
+  ledcAttachPin(PWMB, channel_B);
+
+  digitalWrite(AIN1, LOW);
+  digitalWrite(AIN2, LOW);
+  digitalWrite(BIN1, LOW);
+  digitalWrite(BIN2, LOW);
+}
+
+
+void switchEmergencyStop(){
+  digitalWrite(AIN1, LOW);
+  digitalWrite(AIN2, LOW);
+
+  digitalWrite(BIN1, LOW);
+  digitalWrite(BIN2, LOW);
+}
+
+
+void switchPortCtrlA(float pwmInputA){
+  int pwmIntA = round(pwmInputA * spd_rate_A);
+  if(abs(pwmIntA) < 1e-6){
+    digitalWrite(AIN1, LOW);
+    digitalWrite(AIN2, LOW);
+    return;
+  }
+
+  if(pwmIntA > 0){
+    digitalWrite(AIN1, LOW);
+    digitalWrite(AIN2, HIGH);
+    ledcWrite(channel_A, pwmIntA);
+  }
+  else{
+    digitalWrite(AIN1, HIGH);
+    digitalWrite(AIN2, LOW);
+    ledcWrite(channel_A,-pwmIntA);
+  }
+}
+
+
+void switchPortCtrlB(float pwmInputB){
+  int pwmIntB = round(pwmInputB * spd_rate_B);
+  if(abs(pwmIntB) < 1e-6){
+    digitalWrite(BIN1, LOW);
+    digitalWrite(BIN2, LOW);
+    return;
+  }
+
+  if(pwmIntB > 0){
+    digitalWrite(BIN1, LOW);
+    digitalWrite(BIN2, HIGH);
+    ledcWrite(channel_B, pwmIntB);
+  }
+  else{
+    digitalWrite(BIN1, HIGH);
+    digitalWrite(BIN2, LOW);
+    ledcWrite(channel_B,-pwmIntB);
+  }
+}
+
+
+void switchCtrl(int pwmIntA, int pwmIntB) {
+    switch_pwm_A = pwmIntA;
+    switch_pwm_B = pwmIntB;
+    switchPortCtrlA(switch_pwm_A);
+    switchPortCtrlB(switch_pwm_B);
+}
+
+
+void lightCtrl(int pwmIn) {
+  switch_pwm_A = pwmIn;
+  switchPortCtrlA(-abs(switch_pwm_A));
+}
+
+
+void setSpdRate(float inputL, float inputR) {
+  inputL = abs(inputL);
+  if (inputL > 1) {
+    inputL = 1;
+  }
+  inputR = abs(inputR);
+  if (inputR > 1) {
+    inputR = 1;
+  }
+  spd_rate_A = inputL;
+  spd_rate_B = inputR;
+}
+
+
+void getSpdRate() {
+  jsonInfoHttp.clear();
+  jsonInfoHttp["T"] = CMD_GET_SPD_RATE;
+
+  jsonInfoHttp["L"] = spd_rate_A;
+  jsonInfoHttp["R"] = spd_rate_B;
+
+  String getInfoJsonString;
+  serializeJson(jsonInfoHttp, getInfoJsonString);
+  Serial.println(getInfoJsonString);
+}
+
+
+
+// movtion parts.
+// A-left, B-right
+
+ESP32Encoder encoderA;
+ESP32Encoder encoderB;
+
+static unsigned long lastTime = 0;
+static unsigned long lastLeftSpdTime = 0;
+static unsigned long lastRightSpdTime = 0;
+int lastEncoderA = 0;
+int lastEncoderB = 0;
+
+double speedGetA;
+double speedGetB;
+
+double plusesRate = 3.14159265359 * WHEEL_D / ONE_CIRCLE_PLUSES;
+
+
+void initEncoders() {
+  // if(SET_MOTOR_DIR){
+  //   encoderA.attachHalfQuad(AENCB, AENCA);
+  //   encoderB.attachHalfQuad(BENCB, BENCA);
+  // }else{
+  encoderA.attachHalfQuad(AENCA, AENCB);
+  encoderB.attachHalfQuad(BENCA, BENCB);
+  // }
+  encoderA.setCount(0);
+  encoderB.setCount(0);
+}
+
+void getWheelSpeed() {
+  unsigned long currentTime = micros();
+  long encoderPulsesA = encoderA.getCount();
+  long encoderPulsesB = encoderB.getCount();
+
+  if (!SET_MOTOR_DIR) {
+    speedGetA = (plusesRate * (encoderPulsesA - lastEncoderA)) / ((double)(currentTime - lastTime) / 1000000);
+    speedGetB = (plusesRate * (encoderPulsesB - lastEncoderB)) / ((double)(currentTime - lastTime) / 1000000);
+  } else {
+    speedGetA = (plusesRate * (lastEncoderA - encoderPulsesA)) / ((double)(currentTime - lastTime) / 1000000);
+    speedGetB = (plusesRate * (lastEncoderB - encoderPulsesB)) / ((double)(currentTime - lastTime) / 1000000);
+  }
+  lastEncoderA = encoderPulsesA;
+  lastEncoderB = encoderPulsesB;
+  lastTime = currentTime;
+}
+
+void getLeftSpeed() {
+  unsigned long currentTime = micros();
+  long encoderPulsesA = encoderA.getCount();
+  if (!SET_MOTOR_DIR) {
+    speedGetA = (plusesRate * (encoderPulsesA - lastEncoderA)) / ((double)(currentTime - lastLeftSpdTime) / 1000000);
+  } else {
+    speedGetA = (plusesRate * (lastEncoderA - encoderPulsesA)) / ((double)(currentTime - lastLeftSpdTime) / 1000000);
+  }
+  lastEncoderA = encoderPulsesA;
+  lastLeftSpdTime = currentTime;
+}
+
+void getRightSpeed() {
+  unsigned long currentTime = micros();
+  long encoderPulsesB = encoderB.getCount();
+  if (!SET_MOTOR_DIR) {
+    speedGetB = (plusesRate * (encoderPulsesB - lastEncoderB)) / ((double)(currentTime - lastRightSpdTime) / 1000000);
+  } else {
+    speedGetB = (plusesRate * (lastEncoderB - encoderPulsesB)) / ((double)(currentTime - lastRightSpdTime) / 1000000);
+  }
+  lastEncoderB = encoderPulsesB;
+  lastRightSpdTime = currentTime;
+}
+
+
+
+
+
+// --- PID Controller ---
+
+PID_v2 pidA(__kp, __ki, __kd, PID::Direct);
+PID_v2 pidB(__kp, __ki, __kd, PID::Direct);
+
+double outputA = 0;
+double outputB = 0;
+double setpointA = 0;
+double setpointB = 0;
+
+int setpoint_interval = 200;
+unsigned long setpoint_cmd_recv = millis();
+unsigned long setpoint_last_time = millis();
+float setpointA_buffer;
+float setpointB_buffer;
+float setpointA_last;
+float setpointB_last;
+float change_offset = 0.005;
+bool new_setpoint_flag = false;
+
+void pidControllerInit() {
+  pidA.Start(speedGetA,
+             outputA,
+             setpointA);
+  pidA.SetOutputLimits(-255, 255);
+  pidA.SetMode(PID::Automatic);
+
+  pidB.Start(speedGetB,
+             outputB,
+             setpointB);
+  pidB.SetOutputLimits(-255, 255);
+  pidB.SetMode(PID::Automatic);
+}
+
+void leftCtrl(float pwmInputA){
+  int pwmIntA = round(pwmInputA);
+  if (mainType != 3) {
+    speedGetA = pwmIntA;
+  }
+  if(SET_MOTOR_DIR){
+    if(pwmIntA < 0){
+      digitalWrite(AIN1, HIGH);
+      digitalWrite(AIN2, LOW);
+      ledcWrite(channel_A, abs(pwmIntA));
+    }
+    else{
+      digitalWrite(AIN1, LOW);
+      digitalWrite(AIN2, HIGH);
+      ledcWrite(channel_A, abs(pwmIntA));
+    }
+  }else{
+    if(pwmIntA < 0){
+      digitalWrite(AIN1, LOW);
+      digitalWrite(AIN2, HIGH);
+      ledcWrite(channel_A, abs(pwmIntA));
+    }
+    else{
+      digitalWrite(AIN1, HIGH);
+      digitalWrite(AIN2, LOW);
+      ledcWrite(channel_A, abs(pwmIntA));
+    }
+  }
+}
+
+void rightCtrl(float pwmInputB){
+  int pwmIntB = round(pwmInputB);
+  if (mainType != 3) {
+    speedGetB = pwmIntB;
+  }
+  if(SET_MOTOR_DIR){
+    if(pwmIntB < 0){
+      digitalWrite(BIN1, HIGH);
+      digitalWrite(BIN2, LOW);
+      ledcWrite(channel_B, abs(pwmIntB));
+    }
+    else{
+      digitalWrite(BIN1, LOW);
+      digitalWrite(BIN2, HIGH);
+      ledcWrite(channel_B, abs(pwmIntB));
+    }
+  }else{
+    if(pwmIntB < 0){
+      digitalWrite(BIN1, LOW);
+      digitalWrite(BIN2, HIGH);
+      ledcWrite(channel_B, abs(pwmIntB));
+    }
+    else{
+      digitalWrite(BIN1, HIGH);
+      digitalWrite(BIN2, LOW);
+      ledcWrite(channel_B, abs(pwmIntB));
+    }
+  }
+}
+
+void setGoalSpeed(float inputLeft, float inputRight) {
+  // setpoint_cmd_recv = millis();
+  if (mainType == 3) {
+    usePIDCompute = true;
+
+    if(inputLeft < -2.0 || inputLeft > 2.0){
+      return;
+    }
+
+    if(inputRight < -2.0 || inputRight > 2.0){
+      return;
+    }
+    
+    setpointA = inputLeft*spd_rate_A;
+    setpointB = inputRight*spd_rate_B;
+
+    if (setpointA != setpointA_buffer) {
+      pidA.Setpoint(setpointA);
+      setpointA_buffer = inputLeft;
+    }
+    
+    if (setpointB != setpointB_buffer) {
+      pidB.Setpoint(setpointB);
+      setpointB_buffer = inputRight;
+    }
+  } else {
+    usePIDCompute = false;
+    leftCtrl(inputLeft * 512 * spd_rate_A);
+    rightCtrl(inputRight * 512 * spd_rate_B);
+  }
+}
+
+void pidControllerCompute() {
+  if (!usePIDCompute) {
+    return;
+  }
+
+  outputA = pidA.Run(speedGetA);
+  if (abs(outputA)<THRESHOLD_PWM) {
+    outputA = 0;
+  }
+  if (setpointA == 0 && speedGetA == 0) {
+    outputA = 0;
+  }
+  leftCtrl(outputA);
+
+  outputB = pidB.Run(speedGetB);
+  if (abs(outputB)<THRESHOLD_PWM) {
+    outputB = 0;
+  }
+  if (setpointB == 0 && speedGetB == 0) {
+    outputB = 0;
+  }
+  rightCtrl(outputB);
+}
+
+void LeftPidControllerCompute() {
+  if (!usePIDCompute) {
+    return;
+  }
+
+  outputA = pidA.Run(speedGetA);
+  if (abs(outputA)<THRESHOLD_PWM) {
+    outputA = 0;
+  }
+  if (setpointA == 0 && speedGetA == 0) {
+    outputA = 0;
+  }
+  leftCtrl(outputA);
+}
+
+void RightPidControllerCompute() {
+  if (!usePIDCompute) {
+    return;
+  }
+
+  outputB = pidB.Run(speedGetB);
+  if (abs(outputB)<THRESHOLD_PWM) {
+    outputB = 0;
+  }
+  if (setpointB == 0 && speedGetB == 0) {
+    outputB = 0;
+  }
+  rightCtrl(outputB);
+}
+
+void setPID(float inputP, float inputI, float inputD, float inputLimits) {
+  __kp = inputP;
+  __ki = inputI;
+  __kd = inputD;
+  windup_limits = inputLimits;
+  pidA.SetTunings(__kp, __ki, __kd);
+  pidB.SetTunings(__kp, __ki, __kd);
+}
+
+void rosCtrl(float rosX, float rosZ) {
+  setpointA = rosX - (rosZ * TRACK_WIDTH / 2.0);
+  setpointB = rosX + (rosZ * TRACK_WIDTH / 2.0);
+  setGoalSpeed(setpointA, setpointB);
+}
+
+void heartBeatCtrl() {
+  if (currentTimeMillis - lastCmdRecvTime > HEART_BEAT_DELAY) {
+    if (!heartbeatStopFlag) {
+      heartbeatStopFlag = true;
+      setGoalSpeed(0, 0);
+      // leftCtrl(0);
+      // rightCtrl(0);
+    }
+  }
+}
+
+void changeHeartBeatDelay(int inputCmd) {
+  HEART_BEAT_DELAY = inputCmd;
+}
+
+void mm_settings(byte inputMain, byte inputModule) {
+  mainType = inputMain;
+  moduleType = inputModule;
+
+  if (mainType == 1) {
+    WHEEL_D = 0.0800;
+    ONE_CIRCLE_PLUSES = 2100;
+    TRACK_WIDTH = 0.125;
+    SET_MOTOR_DIR = false; // checked
+    usePIDCompute = false;
+  } else if (mainType == 2) {
+    WHEEL_D = 0.0800;
+    ONE_CIRCLE_PLUSES = 1650;
+    TRACK_WIDTH = 0.172;
+    SET_MOTOR_DIR = true; // checked
+    usePIDCompute = false;
+  } else if (mainType == 3) {
+    WHEEL_D = 0.0523;
+    ONE_CIRCLE_PLUSES = 1092;
+    TRACK_WIDTH = 0.141;
+    SET_MOTOR_DIR = true; // checked
+    usePIDCompute = true;
+  }
+  plusesRate = 3.14159265359 * WHEEL_D / ONE_CIRCLE_PLUSES;
+  // initEncoders();
+
+  if (mainType == 1) {
+    screenLine_2 = "RaspRover";
+  } else if (mainType == 2) {
+    screenLine_2 = "UGV02";
+  } else if (mainType == 3) {
+    screenLine_2 = "UGV01";
+  } 
+
+  if (moduleType == 0) {
+    screenLine_2 += " Null";
+  } else if (moduleType == 1) {
+    screenLine_2 += " Arm";
+  } else if (moduleType == 2) {
+    screenLine_2 += " PT";
+  } 
+}

oled_ctrl.h

@@ -0,0 +1,98 @@
+// <<<<<<<<<<=== === ===SSD1306: 0x3C=== === ===>>>>>>>>>>
+// 0.91inch OLED
+bool screenDefaultMode = true;
+
+unsigned long currentTimeMillis = millis();
+unsigned long lastTimeMillis = millis();
+
+// default
+String screenLine_0;
+String screenLine_1;
+String screenLine_2;
+String screenLine_3;
+
+// custom
+String customLine_0;
+String customLine_1;
+String customLine_2;
+String customLine_3;
+
+// #include <Adafruit_SSD1306.h>
+#define SCREEN_WIDTH   128 // OLED display width, in pixels
+#define SCREEN_HEIGHT  32 // OLED display height, in pixels
+#define OLED_RESET     -1 // Reset pin # (or -1 if sharing Arduino reset pin)
+#define SCREEN_ADDRESS 0x3C ///< See datasheet for Address; 0x3D for 128x64, 0x3C for 128x32
+Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
+
+// init oled ctrl functions.
+void init_oled(){
+  if(!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
+    Serial.println(F("SSD1306 allocation failed"));
+  }
+  display.clearDisplay();
+  display.setTextSize(1);
+  display.setTextColor(SSD1306_WHITE);
+  display.setCursor(0,0);
+  display.display();
+}
+
+
+// Updata all data and flash the screen.
+void oled_update() {
+  display.clearDisplay();
+  display.setCursor(0,0);
+
+  display.println(screenLine_0);
+  display.println(screenLine_1);
+  display.println(screenLine_2);
+  display.println(screenLine_3);
+
+  display.display();
+}
+
+// dev info update on oled.
+void oledInfoUpdate() {
+  currentTimeMillis = millis();
+  if (currentTimeMillis - lastTimeMillis > 10000) {
+    inaDataUpdate();
+    lastTimeMillis = currentTimeMillis;
+  } else {
+    return;
+  }
+  if (!screenDefaultMode) {
+    return;
+  }
+  // inaDataUpdate();
+  screenLine_3 = "V:"+String(loadVoltage_V);
+  oled_update();
+  
+}
+
+// oled ctrl.
+void oledCtrl(byte inputLineNum, String inputMegs) {
+  screenDefaultMode = false;
+  switch (inputLineNum) {
+  case 0: customLine_0 = inputMegs;break;
+  case 1: customLine_1 = inputMegs;break;
+  case 2: customLine_2 = inputMegs;break;
+  case 3: customLine_3 = inputMegs;break;
+  }
+  display.clearDisplay();
+  display.setCursor(0,0);
+
+  display.println(customLine_0);
+  display.println(customLine_1);
+  display.println(customLine_2);
+  display.println(customLine_3);
+
+  display.display();
+}
+
+// set oled as default.
+void setOledDefault(){
+  screenDefaultMode = true;
+  inaDataUpdate();
+  screenLine_3 = "V:"+String(loadVoltage_V);
+  oled_update();
+  lastTimeMillis = currentTimeMillis;
+}

uart_ctrl.h

@@ -0,0 +1,516 @@
+void jsonCmdReceiveHandler(){
+	int cmdType = jsonCmdReceive["T"].as<int>();
+	switch(cmdType){
+	case CMD_SPEED_CTRL:	if (jsonCmdReceive.containsKey("T") &&
+														jsonCmdReceive.containsKey("L") &&
+														jsonCmdReceive.containsKey("R")){
+													if (jsonCmdReceive["L"].is<float>() &&
+															jsonCmdReceive["R"].is<float>()){
+														heartbeatStopFlag = false;
+														lastCmdRecvTime = millis();
+														setGoalSpeed(
+														jsonCmdReceive["L"],
+														jsonCmdReceive["R"]);
+													}
+												} break;
+	case CMD_PWM_INPUT:		usePIDCompute = false;
+												heartbeatStopFlag = false;
+												lastCmdRecvTime = millis();
+												leftCtrl(jsonCmdReceive["L"]);
+												rightCtrl(jsonCmdReceive["R"]);
+												break;
+	case CMD_ROS_CTRL:		rosCtrl(
+												jsonCmdReceive["X"],
+												jsonCmdReceive["Z"]);
+												heartbeatStopFlag = false;
+												lastCmdRecvTime = millis();break;
+	case CMD_SET_MOTOR_PID:
+												setPID(
+												jsonCmdReceive["P"],
+												jsonCmdReceive["I"],
+												jsonCmdReceive["D"],
+												jsonCmdReceive["L"]);break;
+	case CMD_OLED_CTRL:		oledCtrl(
+												jsonCmdReceive["lineNum"],
+												jsonCmdReceive["Text"]);break;
+	case CMD_OLED_DEFAULT:setOledDefault();break;
+	case CMD_MODULE_TYPE:	changeModuleType(
+												jsonCmdReceive["cmd"]);break;
+
+
+
+	case CMD_GET_IMU_DATA:
+												getIMUData();break;
+	case CMD_CALI_IMU_STEP:
+												imuCalibration();break;
+	case CMD_GET_IMU_OFFSET:
+												getIMUOffset();
+												break;
+	case CMD_SET_IMU_OFFSET:
+												setIMUOffset(
+												jsonCmdReceive["x"],
+												jsonCmdReceive["y"],
+												jsonCmdReceive["z"]);break;
+	case CMD_BASE_FEEDBACK:
+												baseInfoFeedback();break;
+	case CMD_BASE_FEEDBACK_FLOW:
+												setBaseInfoFeedbackMode(
+												jsonCmdReceive["cmd"]);break;
+	case CMD_FEEDBACK_FLOW_INTERVAL:
+												setFeedbackFlowInterval(
+												jsonCmdReceive["cmd"]);break;
+	case CMD_UART_ECHO_MODE:
+												setCmdEcho(
+												jsonCmdReceive["cmd"]);break;
+	case CMD_ARM_CTRL_UI: RoArmM2_uiCtrl(
+												jsonCmdReceive["E"],
+												jsonCmdReceive["Z"],
+												jsonCmdReceive["R"]
+												);break;
+                        
+
+
+	case CMD_LED_CTRL:		led_pwm_ctrl(
+												jsonCmdReceive["IO4"],
+												jsonCmdReceive["IO5"]);break;
+	case CMD_GIMBAL_CTRL_SIMPLE:
+												gimbalCtrlSimple(
+												jsonCmdReceive["X"],
+												jsonCmdReceive["Y"],
+												jsonCmdReceive["SPD"],
+												jsonCmdReceive["ACC"]);break;
+	case CMD_GIMBAL_CTRL_MOVE:
+												gimbalCtrlMove(
+												jsonCmdReceive["X"],
+												jsonCmdReceive["Y"],
+												jsonCmdReceive["SX"],
+												jsonCmdReceive["SY"]);break;
+	case CMD_GIMBAL_CTRL_STOP:
+												gimbalCtrlStop();break;
+	case CMD_HEART_BEAT_SET:
+												changeHeartBeatDelay(
+												jsonCmdReceive["cmd"]);break;
+	case CMD_GIMBAL_STEADY:
+												gimbalSteadySet(
+												jsonCmdReceive["s"],
+												jsonCmdReceive["y"]);break;
+	case CMD_SET_SPD_RATE:
+												setSpdRate(
+												jsonCmdReceive["L"],
+												jsonCmdReceive["R"]);break;
+	case CMD_GET_SPD_RATE:
+												getSpdRate();break;
+	case CMD_SAVE_SPD_RATE:
+												saveSpdRate();break;
+	case CMD_GIMBAL_USER_CTRL:
+												gimbalUserCtrl(
+												jsonCmdReceive["X"],
+												jsonCmdReceive["Y"],
+												jsonCmdReceive["SPD"]);break;
+
+
+
+
+	// EoAT type settings.
+	case CMD_EOAT_TYPE:		configEEmodeType(
+												jsonCmdReceive["mode"]);break;
+	case CMD_CONFIG_EOAT: configEoAT(
+												jsonCmdReceive["pos"],
+												jsonCmdReceive["ea"],
+												jsonCmdReceive["eb"]
+												);break;
+
+
+
+	// it moves to goal position directly
+	// with interpolation.
+	case CMD_MOVE_INIT:		RoArmM2_moveInit();break;
+	case CMD_SINGLE_JOINT_CTRL: 
+												RoArmM2_singleJointAbsCtrl(
+												jsonCmdReceive["joint"],
+												jsonCmdReceive["rad"],
+												jsonCmdReceive["spd"],
+												jsonCmdReceive["acc"]
+												);break;
+	case CMD_JOINTS_RAD_CTRL: 
+												RoArmM2_allJointAbsCtrl(
+												jsonCmdReceive["base"],
+												jsonCmdReceive["shoulder"],
+												jsonCmdReceive["elbow"],
+												jsonCmdReceive["hand"],
+												jsonCmdReceive["spd"],
+												jsonCmdReceive["acc"]
+												);break;
+	case CMD_SINGLE_AXIS_CTRL: 
+												RoArmM2_singlePosAbsBesselCtrl(
+												jsonCmdReceive["axis"],
+												jsonCmdReceive["pos"],
+												jsonCmdReceive["spd"]
+												);break;
+	case CMD_XYZT_GOAL_CTRL: 
+												RoArmM2_allPosAbsBesselCtrl(
+												jsonCmdReceive["x"],
+											  jsonCmdReceive["y"],
+											  jsonCmdReceive["z"],
+											  jsonCmdReceive["t"],
+											  jsonCmdReceive["spd"]
+											  );break;
+	case CMD_XYZT_DIRECT_CTRL:
+												RoArmM2_baseCoordinateCtrl(
+												jsonCmdReceive["x"],
+												jsonCmdReceive["y"],
+												jsonCmdReceive["z"],
+												jsonCmdReceive["t"]);
+												RoArmM2_goalPosMove();
+												break;
+	case CMD_SERVO_RAD_FEEDBACK:
+												RoArmM2_getPosByServoFeedback();
+												RoArmM2_infoFeedback();
+												break;
+
+	case CMD_EOAT_HAND_CTRL: 
+												RoArmM2_handJointCtrlRad(1,
+												jsonCmdReceive["cmd"],
+												jsonCmdReceive["spd"],
+												jsonCmdReceive["acc"]
+												);break;
+	case CMD_EOAT_GRAB_TORQUE:
+												RoArmM2_handTorqueCtrl(
+												jsonCmdReceive["tor"]
+												);break;
+
+	case CMD_SET_JOINT_PID:
+												RoArmM2_setJointPID(
+												jsonCmdReceive["joint"],
+												jsonCmdReceive["p"],
+												jsonCmdReceive["i"]
+												);break;
+	case CMD_RESET_PID:		RoArmM2_resetPID();break;
+
+	// set a new x-axis.
+	case CMD_SET_NEW_X: 	setNewAxisX(
+												jsonCmdReceive["xAxisAngle"]
+												);break;
+	case CMD_DELAY_MILLIS:
+												RoArmM2_delayMillis(
+												jsonCmdReceive["cmd"]
+												);break;
+	case CMD_DYNAMIC_ADAPTATION: 
+												RoArmM2_dynamicAdaptation(
+												jsonCmdReceive["mode"],
+												jsonCmdReceive["b"],
+												jsonCmdReceive["s"],
+												jsonCmdReceive["e"],
+												jsonCmdReceive["h"]
+												);break;
+	// this two funcs are NOT for UGV.
+	// case CMD_SWITCH_CTRL: switchCtrl(
+	// 											jsonCmdReceive["pwm_a"],
+	// 											jsonCmdReceive["pwm_b"]
+	// 											);break;
+	// case CMD_LIGHT_CTRL:	lightCtrl(
+	// 											jsonCmdReceive["led"]
+	// 											);break;
+	case CMD_SWITCH_OFF:  switchEmergencyStop();break;
+	case CMD_SINGLE_JOINT_ANGLE:
+												RoArmM2_singleJointAngleCtrl(
+												jsonCmdReceive["joint"],
+												jsonCmdReceive["angle"],
+												jsonCmdReceive["spd"],
+												jsonCmdReceive["acc"]
+												);break;
+	case CMD_JOINTS_ANGLE_CTRL:
+												RoArmM2_allJointsAngleCtrl(
+												jsonCmdReceive["b"],
+												jsonCmdReceive["s"],
+												jsonCmdReceive["e"],
+												jsonCmdReceive["h"],
+												jsonCmdReceive["spd"],
+												jsonCmdReceive["acc"]
+												);break;
+// constant ctrl
+// m: 0 - angle
+//    1 - xyzt
+// cmd: 0 - stop
+// 		  1 - increase
+// 		  2 - decrease
+// {"T":123,"m":0,"axis":0,"cmd":0,"spd":0}
+	case CMD_CONSTANT_CTRL:
+												constantCtrl(
+												jsonCmdReceive["m"],
+												jsonCmdReceive["axis"],
+												jsonCmdReceive["cmd"],
+												jsonCmdReceive["spd"]
+												);break;
+
+
+
+
+	// mission & steps edit & file edit.
+	case CMD_SCAN_FILES:  scanFlashContents();
+												break;
+	case CMD_CREATE_FILE: createFile(
+												jsonCmdReceive["name"],
+												jsonCmdReceive["content"]
+												);break;
+	case CMD_READ_FILE:		readFile(
+												jsonCmdReceive["name"]
+												);break;
+	case CMD_DELETE_FILE: deleteFile(
+												jsonCmdReceive["name"]
+												);break;
+	case CMD_APPEND_LINE:	appendLine(
+												jsonCmdReceive["name"],
+												jsonCmdReceive["content"]
+												);break;
+	case CMD_INSERT_LINE: insertLine(
+												jsonCmdReceive["name"],
+												jsonCmdReceive["lineNum"],
+												jsonCmdReceive["content"]
+												);break;
+	case CMD_REPLACE_LINE:
+												replaceLine(
+												jsonCmdReceive["name"],
+												jsonCmdReceive["lineNum"],
+												jsonCmdReceive["content"]
+												);break;
+	case CMD_READ_LINE:   readSingleLine(
+												jsonCmdReceive["name"],
+												jsonCmdReceive["lineNum"]
+												);break;
+	case CMD_DELETE_LINE: deleteSingleLine(
+												jsonCmdReceive["name"],
+												jsonCmdReceive["lineNum"]
+												);break;
+
+
+	case CMD_TORQUE_CTRL: servoTorqueCtrl(254,
+												jsonCmdReceive["cmd"]);
+												break;
+
+
+	case CMD_CREATE_MISSION:
+												createMission(
+												jsonCmdReceive["name"],
+												jsonCmdReceive["intro"]
+												);break;
+	case CMD_MISSION_CONTENT:
+												missionContent(
+												jsonCmdReceive["name"]
+												);break;
+	case CMD_APPEND_STEP_JSON: 
+												appendStepJson(
+												jsonCmdReceive["name"],
+												jsonCmdReceive["step"]
+												);break;
+	case CMD_APPEND_STEP_FB:
+												appendStepFB(
+												jsonCmdReceive["name"],
+												jsonCmdReceive["spd"]
+												);break;
+	case CMD_APPEND_DELAY:
+												appendDelayCmd(
+												jsonCmdReceive["name"],
+												jsonCmdReceive["delay"]
+												);break;
+	case CMD_INSERT_STEP_JSON:
+												insertStepJson(
+												jsonCmdReceive["name"],
+												jsonCmdReceive["stepNum"],
+												jsonCmdReceive["step"]
+												);break;
+	case CMD_INSERT_STEP_FB:
+												insertStepFB(
+												jsonCmdReceive["name"],
+												jsonCmdReceive["stepNum"],
+												jsonCmdReceive["spd"]
+												);break;
+	case CMD_INSERT_DELAY:
+												insertDelayCmd(
+												jsonCmdReceive["name"],
+												jsonCmdReceive["stepNum"],
+												jsonCmdReceive["spd"]
+												);break;
+	case CMD_REPLACE_STEP_JSON:
+												replaceStepJson(
+												jsonCmdReceive["name"],
+												jsonCmdReceive["stepNum"],
+												jsonCmdReceive["step"]
+												);break;
+	case CMD_REPLACE_STEP_FB:
+												replaceStepFB(
+												jsonCmdReceive["name"],
+												jsonCmdReceive["stepNum"],
+												jsonCmdReceive["spd"]
+												);break;
+	case CMD_REPLACE_DELAY:
+												replaceDelayCmd(
+												jsonCmdReceive["name"],
+												jsonCmdReceive["stepNum"],
+												jsonCmdReceive["delay"]
+												);break;
+	case CMD_DELETE_STEP: deleteStep(
+												jsonCmdReceive["name"],
+												jsonCmdReceive["stepNum"]
+												);break;
+
+	case CMD_MOVE_TO_STEP:
+												moveToStep(
+												jsonCmdReceive["name"],
+												jsonCmdReceive["stepNum"]
+												);break;
+	case CMD_MISSION_PLAY:
+												missionPlay(
+												jsonCmdReceive["name"],
+												jsonCmdReceive["times"]
+												);break;
+
+
+
+	// esp-now settings.
+  case CMD_BROADCAST_FOLLOWER:
+  											changeBroadcastMode(
+  											jsonCmdReceive["mode"],
+  											jsonCmdReceive["mac"]
+  											);break;
+  case CMD_ESP_NOW_CONFIG:
+  											changeEspNowMode(
+  											jsonCmdReceive["mode"]
+  											);break;
+  case CMD_GET_MAC_ADDRESS: 
+  											getThisDevMacAddress();
+  											break;
+  case CMD_ESP_NOW_ADD_FOLLOWER:
+  											registerNewFollowerToPeer(
+  											jsonCmdReceive["mac"]);break;
+  case CMD_ESP_NOW_REMOVE_FOLLOWER:
+  											deleteFollower(
+  											jsonCmdReceive["mac"]);break;
+  case CMD_ESP_NOW_GROUP_CTRL:
+  											espNowGroupSend(
+  											jsonCmdReceive["dev"],
+  											jsonCmdReceive["b"],
+  											jsonCmdReceive["s"],
+  											jsonCmdReceive["e"],
+  											jsonCmdReceive["h"],
+  											jsonCmdReceive["cmd"],
+  											jsonCmdReceive["megs"]
+  											);break;
+  case CMD_ESP_NOW_SINGLE:
+  											espNowSingleDevSend(
+  											jsonCmdReceive["mac"],
+  											jsonCmdReceive["dev"],
+  											jsonCmdReceive["b"],
+  											jsonCmdReceive["s"],
+  											jsonCmdReceive["e"],
+  											jsonCmdReceive["h"],
+  											jsonCmdReceive["cmd"],
+  											jsonCmdReceive["megs"]
+  											);break;
+
+
+
+	// wifi settings.
+	case CMD_WIFI_ON_BOOT: 
+												configWifiModeOnBoot(
+												jsonCmdReceive["cmd"]
+												);break;
+	case CMD_SET_AP: 			wifiModeAP(
+									 			jsonCmdReceive["ssid"],
+									 			jsonCmdReceive["password"]
+									 			);break;
+	case CMD_SET_STA: 		wifiModeSTA(
+												jsonCmdReceive["ssid"],
+												jsonCmdReceive["password"]
+												);break;
+	case CMD_WIFI_APSTA: 	wifiModeAPSTA(
+										 	 	jsonCmdReceive["ap_ssid"],
+											 	jsonCmdReceive["ap_password"],
+											 	jsonCmdReceive["sta_ssid"],
+											 	jsonCmdReceive["sta_password"]
+											 	);break;
+	case CMD_WIFI_INFO: 	wifiStatusFeedback();break;
+	case CMD_WIFI_CONFIG_CREATE_BY_STATUS: 
+												createWifiConfigFileByStatus();break;
+	case CMD_WIFI_CONFIG_CREATE_BY_INPUT: 
+												createWifiConfigFileByInput(
+												jsonCmdReceive["mode"],
+												jsonCmdReceive["ap_ssid"],
+												jsonCmdReceive["ap_password"],
+												jsonCmdReceive["sta_ssid"],
+												jsonCmdReceive["sta_password"]
+												);break;
+	case CMD_WIFI_STOP: 	wifiStop();break;
+
+
+
+	// servo settings.
+	case CMD_SET_SERVO_ID:
+												changeID(
+												jsonCmdReceive["raw"],
+												jsonCmdReceive["new"]
+												);break;
+	case CMD_SET_MIDDLE:  setMiddlePos(
+												jsonCmdReceive["id"]
+												);break;
+	case CMD_SET_SERVO_PID: 
+												setServosPID(
+												jsonCmdReceive["id"],
+												jsonCmdReceive["p"]
+												);break;
+
+	// esp-32 dev ctrl.
+	case CMD_REBOOT: 			esp_restart();break;
+	case CMD_FREE_FLASH_SPACE:
+												freeFlashSpace();break;
+	case CMD_BOOT_MISSION_INFO:
+												missionContent("boot");break;
+	case CMD_RESET_BOOT_MISSION:
+												deleteFile("boot.mission");
+												createFile("boot", "these cmds run automatically at boot.");
+												break;
+	case CMD_NVS_CLEAR:		nvs_flash_erase();
+												delay(1000);
+												nvs_flash_init();
+												break;
+	case CMD_INFO_PRINT:	configInfoPrint(
+												jsonCmdReceive["cmd"]
+												);break;
+	// case CMD_PID_RESET_A: PID_v2 pidA(__kp, __ki, __kd, PID::Direct);
+	// 											PID_v2 pidB(__kp, __ki, __kd, PID::Direct);
+	// 											pidControllerInit();break;
+
+	// mainType & moduleType settings.
+	case CMD_MM_TYPE_SET: mm_settings(
+												jsonCmdReceive["main"],
+												jsonCmdReceive["module"]
+												);
+												break;
+	}
+}
+
+
+void serialCtrl() {
+  static String receivedData;
+
+  while (Serial.available() > 0) {
+    char receivedChar = Serial.read();
+    receivedData += receivedChar;
+
+    // Detect the end of the JSON string based on a specific termination character
+    if (receivedChar == '\n') {
+      // Now we have received the complete JSON string
+      DeserializationError err = deserializeJson(jsonCmdReceive, receivedData);
+      if (err == DeserializationError::Ok) {
+  			if (InfoPrint == 1 && uartCmdEcho) {
+  				Serial.print(receivedData);
+  			}
+        jsonCmdReceiveHandler();
+      } else {
+        // Handle JSON parsing error here
+      }
+      // Reset the receivedData for the next JSON string
+      receivedData = "";
+    }
+  }
+}

ugv_advance.h

@@ -0,0 +1,452 @@
+// advance funcs for RoArm-M2 ctrl
+// place holder.
+void jsonCmdReceiveHandler();
+bool moveToStep(String inputName, int inputStepNum);
+
+
+// mission abort after serial received anything.
+bool serialMissionAbort() {
+	if (Serial.available()) {
+		if (InfoPrint == 1) {Serial.println("[missionPlay abort.]");}
+		return true;
+	} else {
+		return false;
+	}
+}
+
+
+// input the mission name and the intro to create a mission file.
+bool createMission(String inputName, String inputIntro) {
+	jsonInfoSend.clear();
+	jsonInfoSend["name"] = inputName;
+	jsonInfoSend["intro"] = inputIntro;
+
+	String contentBuffer;
+	serializeJson(jsonInfoSend, contentBuffer);
+	
+	jsonInfoSend.clear();
+	return createFile(inputName + ".mission", contentBuffer);
+}
+
+
+// input the mission name and get the total content
+int missionContent(String inputName) {
+	File file = LittleFS.open("/" + inputName + ".mission", "r");
+	if (!file) {
+		Serial.println("file not found.");
+		return -1;
+	}
+
+	Serial.println("---=== File Content ===---");
+	Serial.println("reading file: [" + inputName + "] starts:\n");
+	String mission_intro = file.readStringUntil('\n');
+	Serial.println(mission_intro);
+	
+	jsonInfoHttp.clear();
+	jsonInfoHttp["info"] = "reading mission.";
+	jsonInfoHttp["first_line"] = mission_intro;
+
+	int _LineNum = 0;
+	while (file.available()) {
+		_LineNum++;
+		String line = file.readStringUntil('\n');
+		Serial.print("[StepNum: ");Serial.print(_LineNum);Serial.print(" ] - ");
+		Serial.println(line);
+
+		jsonInfoHttp["StepNum_"+String(_LineNum)] = line;
+	}
+
+	Serial.println("^^^ ^^^ ^^^ reading file: " + inputName + ".mission ends. ^^^ ^^^ ^^^");
+	file.close();
+
+	return _LineNum;
+}
+
+
+
+
+// input the mission name and the step to append 
+// a new step at the end of the mission.
+// using inputStep(String)
+bool appendStepJson(String inputName, String inputStep) {
+	DeserializationError err = deserializeJson(jsonInfoSend, inputStep);
+	if (err == DeserializationError::Ok) {
+		if (InfoPrint == 1) {
+			Serial.println("[json parsing succeed.]");
+		}
+		appendLine(inputName + ".mission", inputStep);
+		jsonInfoSend.clear();
+		return true;
+	} else {
+		jsonInfoSend.clear();
+		if (InfoPrint == 1) {
+			Serial.println("[deserializeJson err]");
+		}
+		return false;
+	}
+}
+
+// input the mission name and the step to append
+// a new step at the end of the mission.
+// using feedback.
+void appendStepFB(String inputName, float inputSpd) {
+	RoArmM2_infoFeedback();
+	jsonInfoSend.clear();
+	jsonInfoSend["T"] = 104;
+	jsonInfoSend["x"] = lastX;
+	jsonInfoSend["y"] = lastY;
+	jsonInfoSend["z"] = lastZ;
+	jsonInfoSend["t"] = lastT;
+	jsonInfoSend["spd"] = inputSpd;
+	String contentBuffer;
+	serializeJson(jsonInfoSend, contentBuffer);
+	appendLine(inputName + ".mission", contentBuffer);
+}
+
+// append a new delay(ms) at the end of the mission.
+void appendDelayCmd(String inputName, int delayTime) {
+	jsonInfoSend.clear();
+	jsonInfoSend["T"] = 111;
+	jsonInfoSend["cmd"] = delayTime;
+	String contentBuffer;
+	serializeJson(jsonInfoSend, contentBuffer);
+	appendLine(inputName + ".mission", contentBuffer);
+}
+
+
+
+
+// insert a new step as the stepNum
+// using the json string input.
+bool insertStepJson(String inputName, int inputStepNum, String inputStep) {
+	DeserializationError err = deserializeJson(jsonInfoSend, inputStep);
+	if (err == DeserializationError::Ok) {
+		if (InfoPrint == 1) {
+			Serial.println("[json parsing succeed.]");
+		}
+		insertLine(inputName + ".mission", inputStepNum + 1, inputStep);
+		jsonInfoSend.clear();
+		return true;
+	} else {
+		jsonInfoSend.clear();
+		if (InfoPrint == 1) {
+			Serial.println("[deserializeJson err]");
+		}
+		return false;
+	}
+}
+
+// insert a new step as the stepNum
+// using the feedback.
+void insertStepFB(String inputName, int inputStepNum, float inputSpd) {
+	RoArmM2_infoFeedback();
+	jsonInfoSend.clear();
+	jsonInfoSend["T"] = 104;
+	jsonInfoSend["x"] = lastX;
+	jsonInfoSend["y"] = lastY;
+	jsonInfoSend["z"] = lastZ;
+	jsonInfoSend["t"] = lastT;
+	jsonInfoSend["spd"] = inputSpd;
+	String contentBuffer;
+	serializeJson(jsonInfoSend, contentBuffer);
+	insertLine(inputName + ".mission", inputStepNum + 1, contentBuffer);
+}
+
+// insert a new delayCmd as the stepNum
+void insertDelayCmd(String inputName, int inputStepNum, int delayTime) {
+	jsonInfoSend.clear();
+	jsonInfoSend["T"] = 111;
+	jsonInfoSend["cmd"] = delayTime;
+	String contentBuffer;
+	serializeJson(jsonInfoSend, contentBuffer);
+	insertLine(inputName + ".mission", inputStepNum + 1, contentBuffer);
+}
+
+
+
+
+// replace the cmd at stepNum.
+// using the step json string input.
+bool replaceStepJson(String inputName, int inputStepNum, String inputStep) {
+	DeserializationError err = deserializeJson(jsonInfoSend, inputStep);
+	if (err == DeserializationError::Ok) {
+		if (InfoPrint == 1) {
+			Serial.println("[json parsing succeed.]");
+		}
+		replaceLine(inputName + ".mission", inputStepNum + 1, inputStep);
+		jsonInfoSend.clear();
+		return true;
+	} else {
+		jsonInfoSend.clear();
+		if (InfoPrint == 1) {
+			Serial.println("[deserializeJson err]");
+		}
+		return false;
+	}
+}
+
+// replace the cmd at stepNum.
+// using feedback.
+void replaceStepFB(String inputName, int inputStepNum, float inputSpd) {
+	RoArmM2_infoFeedback();
+	jsonInfoSend.clear();
+	jsonInfoSend["T"] = 104;
+	jsonInfoSend["x"] = lastX;
+	jsonInfoSend["y"] = lastY;
+	jsonInfoSend["z"] = lastZ;
+	jsonInfoSend["t"] = lastT;
+	jsonInfoSend["spd"] = inputSpd;
+	String contentBuffer;
+	serializeJson(jsonInfoSend, contentBuffer);
+	replaceLine(inputName + ".mission", inputStepNum + 1, contentBuffer);
+}
+
+// replace the cmd at stepNum with delay cmd.
+void replaceDelayCmd(String inputName, int inputStepNum, int delayTime) {
+	jsonInfoSend.clear();
+	jsonInfoSend["T"] = 111;
+	jsonInfoSend["cmd"] = delayTime;
+	String contentBuffer;
+	serializeJson(jsonInfoSend, contentBuffer);
+	replaceLine(inputName + ".mission", inputStepNum + 1, contentBuffer);
+}
+
+
+// delete a step
+void deleteStep(String inputName, int inputStepNum) {
+	deleteSingleLine(inputName + ".mission", inputStepNum + 1);
+}
+
+
+// input the mission name and the stepNum.
+// it will process the cmd.
+bool moveToStep(String inputName, int inputStepNum) {
+	String stepStringBuffer = readSingleLine(inputName + ".mission", inputStepNum + 1);
+	DeserializationError err = deserializeJson(jsonCmdReceive, stepStringBuffer);
+	if (err == DeserializationError::Ok) {
+		if (InfoPrint == 1) {
+			Serial.println("[json parsing succeed.]");
+			Serial.println("[import a step]");
+			Serial.print("[mission name]: ");Serial.println(inputName);
+			Serial.print("[stepNum]: ");Serial.println(inputStepNum);
+			Serial.print("[cmd]: ");Serial.println(stepStringBuffer);
+		}
+		jsonCmdReceiveHandler();
+		if (InfoPrint == 1) {
+			Serial.println("[step finished]");
+		}
+		jsonInfoSend.clear();
+		return true;
+	} else {
+		jsonInfoSend.clear();
+		if (InfoPrint == 1) {
+			Serial.println("[deserializeJson err]");
+		}
+		return false;
+	}
+}
+
+
+// input the mission name and the repeat times.
+// when repeatTimes = -1, it will loop forever.
+// play a mission file.
+void missionPlay(String inputName, int repeatTimes) {
+	int _LineNum = missionContent(inputName);
+	int currentTimes = 0;
+	while (1) {
+		currentTimes++;
+		if (currentTimes > repeatTimes && repeatTimes != -1) {
+			if (InfoPrint == 1) {Serial.println("[missionPlay finished.]");}
+			return;
+		}
+		if (InfoPrint == 1) {
+			Serial.print("---\n[currentTimes: ");Serial.print(currentTimes);
+			Serial.println(" ]");
+		}
+
+		for (int i = 1; i<=_LineNum; i++) {
+			if (serialMissionAbort()) {
+				return;
+			}
+			moveToStep(inputName, i);
+		}
+	}
+}
+
+
+// change EEmode.
+void configEEmodeType(byte inputMode) {
+	EEMode = inputMode;
+	if (inputMode == 0){
+		l3A = ARM_L3_LENGTH_MM_A_0;
+		l3B = ARM_L3_LENGTH_MM_B_0;
+		l3  = sqrt(l3A * l3A + l3B * l3B);
+		t3rad = atan2(l3B, l3A);
+
+		initX = l3A + l2B;
+		initY = 0;
+		initZ = l2A - l3B;
+		initT = M_PI;
+	}
+	else if (inputMode == 1){
+		l3A = ARM_L3_LENGTH_MM_A_1;
+		l3B = ARM_L3_LENGTH_MM_B_1;
+		l3  = sqrt(l3A * l3A + l3B * l3B);
+		t3rad = atan2(l3B, l3A);
+
+		EoAT_A = EoAT_A;
+		EoAT_B = EoAT_B;
+		l4A = ARM_L4_LENGTH_MM_A;
+		l4B = ARM_L4_LENGTH_MM_B;
+		lEA = EoAT_A + ARM_L4_LENGTH_MM_A;
+		lEB = EoAT_B + ARM_L4_LENGTH_MM_B;
+		lE  = sqrt(lEA * lEA + lEB * lEB);
+		tErad = atan2(lEB, lEA);
+
+		initX = l3A + l2B + l4A + EoAT_A;
+		initY = 0;
+		initZ = l2A - l3B - l4B - EoAT_B;
+		initT = M_PI;
+	}
+	goalX = initX;
+	goalY = initY;
+	goalZ = initZ;
+	goalT = initT;
+
+	lastX = goalX;
+	lastY = goalY;
+	lastZ = goalZ;
+	lastT = goalT;
+	RoArmM2_baseCoordinateCtrl(initX, initY, initZ, initT);
+	RoArmM2_goalPosMove();
+}
+
+
+// config the siza of EoAT.
+void configEoAT(byte mountPos, double inputEA, double inputEB) {
+	switch (mountPos) {
+	case 0: ARM_L4_LENGTH_MM_A = 67.85;break;
+	case 1: ARM_L4_LENGTH_MM_A = 64.16;break;
+	case 2: ARM_L4_LENGTH_MM_A = 59.07;break;
+	case 3: ARM_L4_LENGTH_MM_A = 51.07;break;
+	}
+
+	EoAT_A = inputEA;
+	EoAT_B = inputEB;
+
+	l4A = ARM_L4_LENGTH_MM_A;
+	l4B = ARM_L4_LENGTH_MM_B;
+	lEA = EoAT_A + ARM_L4_LENGTH_MM_A;
+	lEB = EoAT_B + ARM_L4_LENGTH_MM_B;
+	lE  = sqrt(lEA * lEA + lEB * lEB);
+	tErad = atan2(lEB, lEA);
+
+	initX = l3A + l2B + l4A + EoAT_A;
+	initY = 0;
+	initZ = l2A - l3B - l4B - EoAT_B;
+	initT = M_PI;
+}
+
+
+// set the InfoPrint.
+void configInfoPrint(byte inputCmd) {
+	switch (inputCmd) {
+	case 0: InfoPrint = 0;
+			break;
+	case 1: InfoPrint = 1;
+			break;
+	case 2: InfoPrint = 2;
+			break;
+	}
+}
+
+
+// set the baseInfoFeedback.
+void setBaseInfoFeedbackMode(bool inputCmd) {
+	if (inputCmd == 1) {
+		baseFeedbackFlow = 1;
+	} else if (inputCmd == 0) {
+		baseFeedbackFlow = 0;
+	}
+}
+
+
+// baseInfoFeedback.
+void baseInfoFeedback() {
+	static unsigned long last_feedback_time;
+	if (millis() - last_feedback_time < feedbackFlowExtraDelay) {
+		return;
+	}
+	
+	last_feedback_time = millis();
+
+	jsonInfoHttp.clear();
+	jsonInfoHttp["T"] = FEEDBACK_BASE_INFO;
+
+	jsonInfoHttp["L"] = speedGetA;
+	jsonInfoHttp["R"] = speedGetB;
+
+	jsonInfoHttp["r"] = icm_roll;
+	jsonInfoHttp["p"] = icm_pitch;
+	jsonInfoHttp["y"] = icm_yaw;
+
+	// jsonInfoHttp["q0"] = qw;
+	// jsonInfoHttp["q1"] = qx;
+	// jsonInfoHttp["q2"] = qy;
+	// jsonInfoHttp["q3"] = qz;
+
+	jsonInfoHttp["temp"] = icm_temp;
+
+	jsonInfoHttp["v"] = loadVoltage_V;
+
+	switch(moduleType) {
+	case 1:
+		jsonInfoHttp["x"] = lastX;
+		jsonInfoHttp["y"] = lastY;
+		jsonInfoHttp["z"] = lastZ;
+		jsonInfoHttp["b"] = radB;
+		jsonInfoHttp["s"] = radS;
+		jsonInfoHttp["e"] = radE;
+		jsonInfoHttp["t"] = lastT;
+		jsonInfoHttp["torB"] = servoFeedback[BASE_SERVO_ID - 11].load;
+		jsonInfoHttp["torS"] = servoFeedback[SHOULDER_DRIVING_SERVO_ID - 11].load - servoFeedback[SHOULDER_DRIVEN_SERVO_ID - 11].load;
+		jsonInfoHttp["torE"] = servoFeedback[ELBOW_SERVO_ID - 11].load;
+		jsonInfoHttp["torH"] = servoFeedback[GRIPPER_SERVO_ID - 11].load;
+		break;
+	case 2:
+		jsonInfoHttp["pan"]  = panAngleCompute(gimbalFeedback[0].pos);
+		jsonInfoHttp["tilt"] = tiltAngleCompute(gimbalFeedback[1].pos);
+		break;
+	}
+
+	String getInfoJsonString;
+	serializeJson(jsonInfoHttp, getInfoJsonString);
+	Serial.println(getInfoJsonString);
+}
+
+
+// change module type.
+void changeModuleType(byte inputCmd) {
+	moduleType = inputCmd;
+}
+
+
+void setFeedbackFlowInterval(int inputCmd) {
+	feedbackFlowExtraDelay = abs(inputCmd);
+}
+
+
+void setCmdEcho(bool inputCmd) {
+	uartCmdEcho = inputCmd;
+}
+
+
+void saveSpdRate() {
+	jsonInfoHttp.clear();
+	jsonInfoHttp["T"] = CMD_SET_SPD_RATE;
+	jsonInfoHttp["L"] = spd_rate_A;
+	jsonInfoHttp["R"] = spd_rate_B;
+	String getInfoJsonString;
+	serializeJson(jsonInfoHttp, getInfoJsonString);
+	appendStepJson("boot", getInfoJsonString);
+}

ugv_config.h

@@ -0,0 +1,378 @@
+// the uart used to control servos.
+// GPIO 18 - S_RXD, GPIO 19 - S_TXD, as default.
+#define RoArmM2_Servo_RXD 18
+#define RoArmM2_Servo_TXD 19
+
+// 2: flow feedback.
+// 1: [default]print debug info in serial.
+// 0: don't print debug info in serial.
+byte InfoPrint = 1;
+
+// devices info:
+// espNowMode: 0 - none
+//             1 - flow-leader(group): sending cmds
+//             2 - flow-leader(single): sending cmds to a single follower
+//             3 - [default]follower: recv cmds
+byte espNowMode = 3;
+
+// set the broadcast ctrl mode.
+// broadcast mac address: FF:FF:FF:FF:FF:FF.
+// true  - [default]it can be controled by broadcast mac address.
+// false - it won't be controled by broadcast mac address.
+bool ctrlByBroadcast = true;
+
+// you can define some whitelist mac addresses here.
+uint8_t mac_whitelist_broadcast[] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
+
+// Multifunction End-Effector Switching System.
+// 0 - end servo as grab.
+// 1 - end servo as a joint moving in vertical plane.
+byte EEMode = 0;
+
+// run new json cmd
+bool runNewJsonCmd = false;
+
+// 1: WAVE ROVER
+// 2: UGV02(UGV)
+// 3: UGV01(UGV)
+byte mainType = 1;
+
+// 0: [Base default] without RoArm-M2 and gimbal.
+// 1: [RoArm default] RoArm-M2 mounted on the UGV.
+// 2: [Gimbal default] Gimbal mounted on the UGV.
+byte moduleType = 0;
+
+// false: gimbal steady mode off.
+//  true: gimbal steady mode on.
+bool steadyMode = false;
+
+// 0: turn off base info feedback flow.
+// 1: [default] turn on base info feedback flow.
+bool baseFeedbackFlow = 0;
+
+String thisMacStr;
+
+#define BASE_JOINT     1
+#define SHOULDER_JOINT 2
+#define ELBOW_JOINT    3
+#define EOAT_JOINT     4
+
+// define servoID
+//   |---[14]---|
+//   ||  |  |  ||
+//   ||        ||
+//   ||  |  |  ||
+//   ||        ||
+//   ||  |  |  ||
+//   || -[15]- ||
+//   ||        ||
+//   ||[13][12]||
+//     |  __  |
+//       [11]
+#define BASE_SERVO_ID    11
+#define SHOULDER_DRIVING_SERVO_ID 12
+#define SHOULDER_DRIVEN_SERVO_ID  13
+#define ELBOW_SERVO_ID   14
+#define GRIPPER_SERVO_ID 15
+
+#define ARM_SERVO_MIDDLE_POS  2047
+#define ARM_SERVO_MIDDLE_ANGLE 180
+#define ARM_SERVO_POS_RANGE   4096
+#define ARM_SERVO_ANGLE_RANGE  360
+#define ARM_SERVO_INIT_SPEED   600
+#define ARM_SERVO_INIT_ACC      20
+
+#define ARM_L1_LENGTH_MM    126.06
+#define ARM_L2_LENGTH_MM_A  236.82
+#define ARM_L2_LENGTH_MM_B	30.00 
+#define ARM_L3_LENGTH_MM_A_0	280.15
+#define ARM_L3_LENGTH_MM_B_0	1.73
+
+// 	  TYPE:0
+//    -------L3A-----------O==L2B===
+//    |                    ^       ||
+//   L3B                   |       ||
+//    |              ELBOW_JOINT   ||
+//                                L2A
+//                                 ||
+//                                 ||
+//                                 ||
+//               SHOULDER_JOINT -> OO
+//                                [||]
+//                                 L1
+//                                [||]
+//                   BASE_JOINT -> X
+double l1  = ARM_L1_LENGTH_MM;
+double l2A = ARM_L2_LENGTH_MM_A;
+double l2B = ARM_L2_LENGTH_MM_B;
+double l2  = sqrt(l2A * l2A + l2B * l2B);
+double t2rad = atan2(l2B, l2A);
+double l3A = ARM_L3_LENGTH_MM_A_0;
+double l3B = ARM_L3_LENGTH_MM_B_0;
+double l3  = sqrt(l3A * l3A + l3B * l3B);
+double t3rad = atan2(l3B, l3A);
+
+
+#define ARM_L3_LENGTH_MM_A_1	215.99
+#define ARM_L3_LENGTH_MM_B_1	0
+
+// edge
+double ARM_L4_LENGTH_MM_A =	67.85;
+
+// D-3.2
+// double ARM_L4_LENGTH_MM_A =	64.16;
+
+// D-4.2
+// double ARM_L4_LENGTH_MM_A =	59.07;
+
+// D-10.2
+// double ARM_L4_LENGTH_MM_A =	51.07;
+
+#define ARM_L4_LENGTH_MM_B  5.98
+
+//    TYPE:1
+//                   -------L3A-----------O==L2B===
+//                   |                    ^       ||
+//                  L3B                   |       ||
+//                   |              ELBOW_JOINT   ||
+//          ---L4A---O                           L2A
+//          |                                     ||
+//     |   L4B                                    ||
+//   /      |                                     ||
+// 180°X-EA-X                   SHOULDER_JOINT -> OO
+//   \ |                                         [||]
+//    EB                                          L1
+//     |                                         [||]
+//    --------                      BASE_JOINT -> XX
+
+// 		\  T:210°
+// 		 \
+//  	  EB
+//   	   \
+// 		-----------
+
+double EoAT_A = 0;
+double EoAT_B = 0;
+double l4A = ARM_L4_LENGTH_MM_A;
+double l4B = ARM_L4_LENGTH_MM_B;
+double lEA = EoAT_A + ARM_L4_LENGTH_MM_A;
+double lEB = EoAT_B + ARM_L4_LENGTH_MM_B;
+double lE  = sqrt(lEA * lEA + lEB * lEB);
+double tErad = atan2(lEB, lEA);
+
+
+double initX = l3A+l2B; //
+double initY = 0;
+double initZ = l2A-l3B;
+double initT = M_PI;
+
+double goalX = initX;
+double goalY = initY;
+double goalZ = initZ;
+double goalT = initT;
+
+double lastX = goalX;
+double lastY = goalY;
+double lastZ = goalZ;
+double lastT = goalT;
+
+double base_r;
+
+double delta_x;
+double delta_y;
+
+double beta_x;
+double beta_y;
+
+double radB;
+double radS;
+double radE;
+double radG;
+
+#define MAX_SERVO_ID 32 // MAX:253
+
+// the uart used to control servos.
+// GPIO 18 - S_RXD, GPIO 19 - S_TXD, as default.
+#define S_RXD 18
+#define S_TXD 19
+
+double BASE_JOINT_RAD = 0;
+double SHOULDER_JOINT_RAD = 0;
+double ELBOW_JOINT_RAD = M_PI/2;
+double EOAT_JOINT_RAD = M_PI;
+double EOAT_JOINT_RAD_BUFFER;
+
+double BASE_JOINT_ANG  = 0;
+double SHOULDER_JOINT_ANG = 0;
+double ELBOW_JOINT_ANG = 90.0;
+double EOAT_JOINT_ANG  = 180.0;
+
+// true: torqueLock ON, servo produces torque.
+// false: torqueLock OFF, servo release torque.
+bool RoArmM2_torqueLock = true;
+bool emergencyStopFlag = false;
+bool newCmdReceived = false;
+
+bool nanIK;
+
+bool RoArmM2_initCheckSucceed  = false;
+// bool RoArmM2_initCheckSucceed   = true;
+
+// // // args for syncWritePos.
+u8  servoID[5] = {11, 12, 13, 14, 15};
+s16 goalPos[5] = {2047, 2047, 2047, 2047, 2047};
+u16 moveSpd[5] = {0, 0, 0, 0, 0};
+u8  moveAcc[5] = {ARM_SERVO_INIT_ACC,
+			      ARM_SERVO_INIT_ACC,
+			      ARM_SERVO_INIT_ACC,
+			      ARM_SERVO_INIT_ACC,
+			      ARM_SERVO_INIT_ACC};
+
+
+double ARM_BASE_LIMIT_MIN_RAD     = -M_PI/2;
+double ARM_BASE_LIMIT_MAX_RAD     =  M_PI/2;
+
+double ARM_SHOULDER_LIMIT_MIN_RAD = -M_PI/2;
+double ARM_SHOULDER_LIMIT_MAX_RAD =  M_PI/2;
+
+double ARM_ELBOW_LIMIT_MIN_RAD    = -M_PI/2;
+double ARM_ELBOW_LIMIT_MAX_RAD    =  M_PI/2;
+
+double ARM_GRIPPER_LIMIT_MIN_RAD  = -M_PI/2;
+double ARM_GRIPPER_LIMIT_MAX_RAD  =  M_PI/2;
+
+
+// --- --- --- Pneumatic Components && Lights --- --- ---
+
+const uint16_t ANALOG_WRITE_BITS = 8;
+const uint16_t MAX_PWM = pow(2, ANALOG_WRITE_BITS)-1;
+const uint16_t MIN_PWM = MAX_PWM/4;
+
+#define PWMA 25         // Motor A PWM control  
+#define AIN2 17         // Motor A input 2     
+#define AIN1 21         // Motor A input 1     
+#define BIN1 22         // Motor B input 1       
+#define BIN2 23         // Motor B input 2       
+#define PWMB 26         // Motor B PWM control  
+
+#define AENCA 35        // Encoder A input      
+#define AENCB 34
+
+#define BENCB 16        // Encoder B input     
+#define BENCA 27
+
+int freq = 100000;
+int channel_A = 5;
+int channel_B = 6;
+
+
+// --- --- --- Bus Servo Settings --- --- ---
+
+#define ST_PID_P_ADDR 21
+#define ST_PID_D_ADDR 22
+#define ST_PID_I_ADDR 23
+
+#define ST_PID_ROARM_P   16
+#define ST_PID_DEFAULT_P 32
+
+#define ST_TORQUE_MAX 1000
+#define ST_TORQUE_MIN 50
+
+
+// --- --- --- i2c Settings --- --- ---
+
+#define S_SCL   33
+#define S_SDA   32
+
+
+//  --- --- --- web / constant moving --- --- ---
+
+#define MOVE_STOP 0
+#define MOVE_INCREASE 1
+#define MOVE_DECREASE 2
+
+#define CONST_ANGLE 0
+#define CONST_XYZT  1
+
+float const_spd;
+byte  const_mode;
+
+byte const_cmd_base_x;
+byte const_cmd_shoulder_y;
+byte const_cmd_elbow_z;
+byte const_cmd_eoat_t;
+
+float const_goal_base = BASE_JOINT_ANG;
+float const_goal_shoulder = SHOULDER_JOINT_ANG;
+float const_goal_elbow = ELBOW_JOINT_ANG;
+float const_goal_eoat = EOAT_JOINT_ANG;
+
+unsigned long prev_time = 0;
+
+String jsonFeedbackWeb = "";
+
+
+//  --- --- --- pid controller --- --- ---
+
+float __kp = 20.0;
+float __ki = 2000.0;
+float __kd = 0;
+float windup_limits = 255;
+
+
+//  --- --- --- ugv base --- --- ---
+
+#define THRESHOLD_PWM 23
+
+// mainType:01 RaspRover
+// #define WHEEL_D 0.0800
+// #define ONE_CIRCLE_PLUSES	2100
+// #define TRACK_WIDTH	0.125
+// #define SET_MOTOR_DIR false
+
+// mainType:02 UGV Rover
+// #define WHEEL_D 0.0800
+// #define ONE_CIRCLE_PLUSES	1650
+// #define TRACK_WIDTH	0.172
+// #define SET_MOTOR_DIR false
+
+// mainType:03 UGV Beast
+// #define WHEEL_D	0.0523
+// #define ONE_CIRCLE_PLUSES	1092
+// #define TRACK_WIDTH	0.141
+// #define SET_MOTOR_DIR true
+
+double WHEEL_D = 0.0800;
+int ONE_CIRCLE_PLUSES = 1650;
+double TRACK_WIDTH = 0.172;
+bool SET_MOTOR_DIR = false;
+
+
+#define IO4_PIN 4
+#define IO5_PIN 5
+
+int IO4_CH = 7;
+int IO5_CH = 8;
+
+const uint16_t FREQ = 200;
+
+int feedbackFlowExtraDelay = 0;
+bool uartCmdEcho = 1;
+
+#define GIMBAL_PAN_ID  2
+#define GIMBAL_TILT_ID 1
+
+#define SERVO_STOP_DELAY 3
+
+int HEART_BEAT_DELAY = 3000;
+unsigned long lastCmdRecvTime = millis();
+
+
+// --- --- --- ugv imu --- --- ---
+double icm_pitch, icm_roll, icm_yaw, icm_temp;
+unsigned long last_imu_update = 0;
+
+double qw, qx, qy, qz;
+double ax, ay, az;
+double mx, my, mz;
+double gx, gy, gz;

ugv_led_ctrl.h

@@ -0,0 +1,15 @@
+void led_pin_init(){
+  pinMode(IO4_PIN, OUTPUT);
+  pinMode(IO5_PIN, OUTPUT);
+
+  ledcSetup(IO4_CH, FREQ, ANALOG_WRITE_BITS);
+  ledcSetup(IO5_CH, FREQ, ANALOG_WRITE_BITS);
+
+  ledcAttachPin(IO4_PIN, IO4_CH);
+  ledcAttachPin(IO5_PIN, IO5_CH);
+}
+
+void led_pwm_ctrl(int io4Input, int io5Input) {
+  ledcWrite(IO4_CH, constrain(io4Input, 0, 255));
+  ledcWrite(IO5_CH, constrain(io5Input, 0, 255));
+}

wifi_ctrl.h

@@ -0,0 +1,398 @@
+// wifi ctrl functions.
+// you can refer to this website below to upload a config file to ESP32 Flash.
+// https://randomnerdtutorials.com/install-esp32-filesystem-uploader-arduino-ide/
+
+// libraries:
+// #include <LittleFS.h>
+// #include <WIFI.h>
+// #include <ArduinoJson.h>
+
+// you need to init Serial.
+// bool InfoPrint = true;
+
+// wifi config
+// wifi mode on boot.
+// 0: OFF (you need to use uart-command or upload a new wifiConfig.json to turn it on again)
+// 1: AP (default mode as a brand new product)
+// 2: STA
+// 3: AP+STA (default mode after first wifi connection succeed)
+byte WIFI_MODE_ON_BOOT = 1;
+const char* sta_ssid = "none";
+const char* sta_password = "none";
+const char* ap_ssid = "UGV";
+const char* ap_password = "12345678";
+
+// true: change the WIFI_MODE_ON_BOOT to 3 when first STA mode succeed.
+bool defaultModeToAPSTA = true;
+
+// wifiConfig.yaml example:
+// wifi_mode_on_boot:3
+// sta_ssid:"WIFI_NAME"
+// sta_ssid:"WIFI_PASSWORD"
+// ap_ssid:"WIFI_NAME"
+// ap_ssid:"WIFI_PASSWORD"
+File wifiConfigYaml;
+
+
+// other args:
+unsigned long connectionStartTime;
+unsigned long connectionTimeout = 15000;
+byte WIFI_CURRENT_MODE = -1;
+IPAddress localIP;
+DynamicJsonDocument wifiDoc(256);
+bool wifiConfigFound = false;
+
+
+// update oled accroding to wifi settings.
+void updateOledWifiInfo() {
+  switch(WIFI_CURRENT_MODE) {
+  case 0: 
+    screenLine_0 = "AP: OFF";
+    screenLine_1 = "ST: OFF";
+    break;
+  case 1:
+    screenLine_0 = String("AP:") + ap_ssid;
+    screenLine_1 = "ST: OFF";
+    break;
+  case 2:
+    screenLine_0 = "AP: OFF";
+    screenLine_1 = String("ST:") + localIP.toString();
+    break;
+  case 3:
+    screenLine_0 = String("AP:") + ap_ssid;
+    screenLine_1 = String("ST:") + localIP.toString();
+    break;
+  }
+  oled_update();
+}
+
+
+// load the wifiConfig.json form Flash.
+// the file name is wifiConfig.json in root path.
+bool loadWifiConfig() {
+	wifiConfigYaml = LittleFS.open("/wifiConfig.json", "r");
+	if (wifiConfigYaml) {
+		if (InfoPrint == 1) {Serial.println("/wifiConfig.json load succeed.");}
+		
+		String line = wifiConfigYaml.readStringUntil('\n');
+
+		// parse the YAML file using ArduinoJson.
+		deserializeJson(wifiDoc, line);
+
+		// read configuration values.
+		WIFI_MODE_ON_BOOT = wifiDoc["wifi_mode_on_boot"];
+		sta_ssid = wifiDoc["sta_ssid"];
+		sta_password = wifiDoc["sta_password"];
+		ap_ssid = wifiDoc["ap_ssid"];
+		ap_password = wifiDoc["ap_password"];
+
+		if (InfoPrint == 1) {
+			Serial.println(line);
+		}
+
+		wifiConfigYaml.close();
+		wifiConfigFound = true;
+		jsonInfoHttp.clear();
+  	jsonInfoHttp["ip"] = "/wifiConfig.json load succeed.";
+ 		jsonInfoHttp["wifi_mode_on_boot"] = WIFI_MODE_ON_BOOT;
+ 		jsonInfoHttp["sta_ssid"] = sta_ssid;
+ 		jsonInfoHttp["sta_password"] = sta_password;
+ 		jsonInfoHttp["ap_ssid"] = ap_ssid;
+ 		jsonInfoHttp["ap_password"] = ap_password;
+		return true;
+
+	} else {
+		if (InfoPrint == 1) {Serial.println("cound not found wifiConfig.json.");}
+		wifiConfigFound = false;
+		return false;
+	}
+}
+
+
+// get the ip address.
+IPAddress getIPAddress(byte inputMode) {
+	localIP = WiFi.localIP();
+	if (InfoPrint == 1) {
+		Serial.print("IP: ");
+		Serial.println(localIP.toString());
+	}
+
+	jsonInfoHttp.clear();
+  jsonInfoHttp["ip"] = localIP.toString();
+	return localIP;
+}
+
+
+// create a wifiConfig.json file
+// from the args already be using.
+bool createWifiConfigFileByStatus() {
+	if (WIFI_MODE_ON_BOOT != 0 || WIFI_MODE_ON_BOOT != -1){
+		wifiDoc.clear();
+		wifiDoc["wifi_mode_on_boot"] = WIFI_MODE_ON_BOOT;
+		wifiDoc["sta_ssid"] = sta_ssid;
+		wifiDoc["sta_password"] = sta_password;
+		wifiDoc["ap_ssid"] = ap_ssid;
+		wifiDoc["ap_password"] = ap_password;
+
+		File configFile = LittleFS.open("/wifiConfig.json", "w");
+		if (configFile) {
+			serializeJson(wifiDoc, configFile);
+			configFile.close();
+			if (InfoPrint == 1) {
+				Serial.println("/wifiConfig.json created.");
+			}
+			jsonInfoHttp.clear();
+  		jsonInfoHttp["info"] = "/wifiConfig.json created.";
+			jsonInfoHttp["wifi_mode_on_boot"] = WIFI_MODE_ON_BOOT;
+			jsonInfoHttp["sta_ssid"] = sta_ssid;
+			jsonInfoHttp["sta_password"] = sta_password;
+			jsonInfoHttp["ap_ssid"] = ap_ssid;
+			jsonInfoHttp["ap_password"] = ap_password;
+			return true;
+		} else {
+			jsonInfoHttp.clear();
+  		jsonInfoHttp["info"] = "/wifiConfig.json open failed.";
+			return false;
+		}
+	} else {
+		jsonInfoHttp.clear();
+  	jsonInfoHttp["info"] = "not for this wifi_mode_on_boot.";
+		return false;
+	}
+}
+
+
+// set wifi as AP mode.
+bool wifiModeAP(const char* input_ssid, const char* input_password) {
+	WiFi.disconnect();
+	if (InfoPrint == 1) {Serial.println("wifi mode on boot: AP");}
+	// WiFi.mode(WIFI_AP);
+	WiFi.mode(WIFI_AP_STA);
+	WiFi.softAP(input_ssid, input_password);
+	if (InfoPrint == 1) {
+		Serial.println("AP mode starts...");
+		Serial.print("SSID: ");
+		Serial.println(input_ssid);
+		Serial.print("Password: ");
+		Serial.println(input_password);
+		Serial.println("AP Address: 192.168.4.1");
+	}
+	WIFI_CURRENT_MODE = 1;
+	localIP = WiFi.localIP();
+	ap_ssid = input_ssid;
+	ap_password = input_password;
+
+	updateOledWifiInfo();
+
+	jsonInfoHttp.clear();
+  jsonInfoHttp["info"] = "AP mode starts";
+  jsonInfoHttp["ap_ssid"] = ap_ssid;
+  jsonInfoHttp["ap_password"] = ap_password;
+
+	return true;
+}
+
+
+// set wifi as STA mode.
+bool wifiModeSTA(const char* input_ssid, const char* input_password) {
+	WiFi.disconnect();
+	if (InfoPrint == 1) {Serial.println("wifi mode on boot: STA");}
+	// WiFi.mode(WIFI_STA);
+	WiFi.mode(WIFI_AP_STA);
+	WiFi.begin(input_ssid, input_password);
+	connectionStartTime = millis();
+
+	if (InfoPrint == 1) {Serial.println("STA mode starts: connecting to ");
+					Serial.println(input_ssid);}
+	while (WiFi.status() != WL_CONNECTED) {
+		unsigned long currentTime = millis();
+		if (InfoPrint == 1) {Serial.print(".");}
+		delay(500);
+
+		if (currentTime - connectionStartTime >= connectionTimeout) {
+			WIFI_CURRENT_MODE = -1;
+			if (InfoPrint == 1) {Serial.println(".");Serial.println("STA connection timeout.");}
+			wifiModeAP(ap_ssid, ap_password);
+			updateOledWifiInfo();
+
+			jsonInfoHttp.clear();
+			jsonInfoHttp["info"] = "STA connection timeout.";
+
+			return false;
+			break;
+		}
+	}
+
+	if (InfoPrint == 1) {Serial.println(".");Serial.println("STA connection succeed.");}
+	WIFI_CURRENT_MODE = 2;
+	getIPAddress(WIFI_CURRENT_MODE);
+	sta_ssid = input_ssid;
+	sta_password = input_password;
+
+	jsonInfoHttp.clear();
+	jsonInfoHttp["info"] = "STA connection succeed.";
+	jsonInfoHttp["wifi_mode_on_boot"] = WIFI_MODE_ON_BOOT;
+	jsonInfoHttp["sta_ssid"] = sta_ssid;
+	jsonInfoHttp["sta_password"] = sta_password;
+	jsonInfoHttp["ap_ssid"] = ap_ssid;
+	jsonInfoHttp["ap_password"] = ap_password;
+
+	if (defaultModeToAPSTA && !wifiConfigFound) {
+		WIFI_MODE_ON_BOOT = 3;
+		if (InfoPrint == 1) {Serial.println("[default] wifi mode on boot: AP+STA");}
+		jsonInfoHttp["info"] = "[default] wifi mode on boot: AP+STA";
+		createWifiConfigFileByStatus();
+	}
+	updateOledWifiInfo();
+
+	return true;
+}
+
+
+// set wifi as AP+STA mode.
+bool wifiModeAPSTA(const char* input_ap_ssid, const char* input_ap_password, const char* input_sta_ssid, const char* input_sta_password) {
+	WiFi.disconnect();
+	if (InfoPrint == 1) {Serial.println("wifi mode on boot: AP+STA");}
+	WiFi.mode(WIFI_AP_STA);
+	WiFi.softAP(input_ap_ssid, input_ap_password);
+	if (InfoPrint == 1) {
+		Serial.println("AP/AP+STA mode starts...");
+		Serial.print("AP SSID: ");
+		Serial.println(input_ap_ssid);
+		Serial.print("AP Password: ");
+		Serial.println(input_ap_password);
+		Serial.println("AP Address: 192.168.4.1");
+	}
+	ap_ssid = input_ap_ssid;
+	ap_password = input_ap_password;
+	
+	WiFi.begin(input_sta_ssid, input_sta_password);
+	connectionStartTime = millis();
+
+	if (InfoPrint == 1) {Serial.print("STA/AP+STA mode starts: connecting to ");
+					Serial.println(input_sta_ssid);}
+	while (WiFi.status() != WL_CONNECTED) {
+		unsigned long currentTime = millis();
+		if (InfoPrint == 1) {Serial.print(".");}
+		delay(500);
+
+		if (currentTime - connectionStartTime >= connectionTimeout) {
+			WIFI_CURRENT_MODE = -1;
+			if (InfoPrint == 1) {Serial.println(".");Serial.println("STA connection timeout.");}
+			wifiModeAP(ap_ssid, ap_password);
+			updateOledWifiInfo();
+
+			jsonInfoHttp.clear();
+			jsonInfoHttp["info"] = "STA connection timeout.";
+
+			return false;
+			break;
+		}
+	}
+
+	if (InfoPrint == 1) {Serial.println("STA connection succeed.");}
+	WIFI_CURRENT_MODE = 3;
+	getIPAddress(WIFI_CURRENT_MODE);
+	sta_ssid = input_sta_ssid;
+	sta_password = input_sta_password;
+	if (defaultModeToAPSTA && !wifiConfigFound) {
+		WIFI_MODE_ON_BOOT = 3;
+		if (InfoPrint == 1) {Serial.println("[default] wifi mode on boot: AP+STA");}
+		createWifiConfigFileByStatus();
+	}
+	updateOledWifiInfo();
+
+	jsonInfoHttp.clear();
+	jsonInfoHttp["info"] = "STA connection succeed.";
+	jsonInfoHttp["wifi_mode_on_boot"] = WIFI_MODE_ON_BOOT;
+	jsonInfoHttp["sta_ssid"] = sta_ssid;
+	jsonInfoHttp["sta_password"] = sta_password;
+	jsonInfoHttp["ap_ssid"] = ap_ssid;
+	jsonInfoHttp["ap_password"] = ap_password;
+	
+	return true;
+}
+
+
+// disconnect wifi.
+void wifiStop() {
+	WiFi.disconnect();
+	WIFI_CURRENT_MODE = 0;
+	WiFi.mode(WIFI_AP_STA);
+	updateOledWifiInfo();
+}
+
+
+// wifi mode on boot starts.
+bool wifiModeOnBoot() {
+	bool funcStatus = false;
+	switch(WIFI_MODE_ON_BOOT) {
+	case 0: 
+		if (InfoPrint == 1) {
+			Serial.println("wifi mode on boot: OFF");
+		}
+		funcStatus = true;
+		WIFI_CURRENT_MODE = 0;
+		WiFi.mode(WIFI_AP_STA);
+		break;
+	case 1:
+		funcStatus = wifiModeAP(ap_ssid, ap_password);
+		break;
+	case 2:
+		funcStatus = wifiModeSTA(sta_ssid, sta_password);
+		break;
+	case 3:
+		funcStatus = wifiModeAPSTA(ap_ssid, ap_password, sta_ssid, sta_password);
+		break;
+	}
+	return funcStatus;
+}
+
+
+// change the WIFI_MODE_ON_BOOT.
+void configWifiModeOnBoot(byte inputMode) {
+	WIFI_MODE_ON_BOOT = inputMode;
+	if (InfoPrint == 1) {
+		Serial.print("wifi_mode_on_boot: ");
+		Serial.println(WIFI_MODE_ON_BOOT);
+	}
+	createWifiConfigFileByStatus();
+}
+
+
+// create a wifiConfig.json file
+// from the args input.
+void createWifiConfigFileByInput(byte inputMode, const char* inputApSsid, const char* inputApPassword, const char* inputStaSsid, const char* inputStaPassword) {
+	WIFI_MODE_ON_BOOT = inputMode;
+	wifiModeAPSTA(inputApSsid, inputApPassword, inputStaSsid, inputStaPassword);
+	if (InfoPrint == 1) {
+		Serial.print("wifi_mode_on_boot: ");
+		Serial.println(WIFI_MODE_ON_BOOT);
+	}
+	createWifiConfigFileByStatus();
+}
+
+
+// wifi information feedback.
+void wifiStatusFeedback() {
+	wifiDoc["ip"] = localIP.toString();
+	wifiDoc["rssi"] = WiFi.RSSI();
+	serializeJson(wifiDoc, Serial);
+
+	jsonInfoHttp.clear();
+	jsonInfoHttp["ip"] = wifiDoc["ip"];
+	jsonInfoHttp["rssi"] = wifiDoc["rssi"];
+	jsonInfoHttp["wifi_mode_on_boot"] = WIFI_MODE_ON_BOOT;
+	jsonInfoHttp["sta_ssid"] = sta_ssid;
+	jsonInfoHttp["sta_password"] = sta_password;
+	jsonInfoHttp["ap_ssid"] = ap_ssid;
+	jsonInfoHttp["ap_password"] = ap_password;
+	jsonInfoHttp["mac"] = thisMacStr;
+}
+
+
+// wifi init.
+void initWifi() {
+	loadWifiConfig();
+	wifiModeOnBoot();
+}