From bdd2b4315bc5e5d7786f8a439305856d02304ccf Mon Sep 17 00:00:00 2001 From: Joshua Sacherer Date: Thu, 23 Apr 2026 17:52:46 +0200 Subject: [PATCH] Initial import of WAVE_ROVER_V1.0 --- .gitignore | 7 + AK09918.cpp | 285 +++++++++ AK09918.h | 149 +++++ IMU.cpp | 359 ++++++++++++ IMU.h | 35 ++ IMU_ctrl.h | 103 ++++ QMI8658.cpp | 559 ++++++++++++++++++ QMI8658.h | 102 ++++ QMI8658reg.h | 344 +++++++++++ README.md | 13 + RoArm-M2_module.h | 1303 ++++++++++++++++++++++++++++++++++++++++++ WAVE_ROVER_V1.0.ino | 267 +++++++++ battery_ctrl.h | 28 + data/devConfig.json | 1 + data/wifiConfig.json | 1 + esp_now_ctrl.h | 430 ++++++++++++++ files_ctrl.h | 319 +++++++++++ gimbal_module.h | 198 +++++++ http_server.h | 31 + json_cmd.h | 575 +++++++++++++++++++ movtion_module.h | 449 +++++++++++++++ oled_ctrl.h | 98 ++++ uart_ctrl.h | 516 +++++++++++++++++ ugv_advance.h | 452 +++++++++++++++ ugv_config.h | 378 ++++++++++++ ugv_led_ctrl.h | 15 + web_page.h | 1034 +++++++++++++++++++++++++++++++++ wifi_ctrl.h | 398 +++++++++++++ 28 files changed, 8449 insertions(+) create mode 100644 .gitignore create mode 100644 AK09918.cpp create mode 100644 AK09918.h create mode 100644 IMU.cpp create mode 100644 IMU.h create mode 100644 IMU_ctrl.h create mode 100644 QMI8658.cpp create mode 100644 QMI8658.h create mode 100644 QMI8658reg.h create mode 100644 README.md create mode 100644 RoArm-M2_module.h create mode 100644 WAVE_ROVER_V1.0.ino create mode 100644 battery_ctrl.h create mode 100644 data/devConfig.json create mode 100644 data/wifiConfig.json create mode 100644 esp_now_ctrl.h create mode 100644 files_ctrl.h create mode 100644 gimbal_module.h create mode 100644 http_server.h create mode 100644 json_cmd.h create mode 100644 movtion_module.h create mode 100644 oled_ctrl.h create mode 100644 uart_ctrl.h create mode 100644 ugv_advance.h create mode 100644 ugv_config.h create mode 100644 ugv_led_ctrl.h create mode 100644 web_page.h create mode 100644 wifi_ctrl.h diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..91c894f --- /dev/null +++ b/.gitignore @@ -0,0 +1,7 @@ +.DS_Store +build/ +*.elf +*.map +*.bin +*.hex +.arduino/ diff --git a/AK09918.cpp b/AK09918.cpp new file mode 100644 index 0000000..569ed57 --- /dev/null +++ b/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; +} + diff --git a/AK09918.h b/AK09918.h new file mode 100644 index 0000000..b14899f --- /dev/null +++ b/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 +#include + +/*************************************************************** + 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__ \ No newline at end of file diff --git a/IMU.cpp b/IMU.cpp new file mode 100644 index 0000000..eba7d29 --- /dev/null +++ b/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; +} diff --git a/IMU.h b/IMU.h new file mode 100644 index 0000000..9bb7133 --- /dev/null +++ b/IMU.h @@ -0,0 +1,35 @@ +#ifndef _IMU_H_ +#define _IMU_H_ + +#include "AK09918.h" +#include "QMI8658.h" +#include +#include + +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 diff --git a/IMU_ctrl.h b/IMU_ctrl.h new file mode 100644 index 0000000..9ac9616 --- /dev/null +++ b/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(); +} diff --git a/QMI8658.cpp b/QMI8658.cpp new file mode 100644 index 0000000..a713d55 --- /dev/null +++ b/QMI8658.cpp @@ -0,0 +1,559 @@ +#include "QMI8658.h" + +#include +#include + +#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; + +} + diff --git a/QMI8658.h b/QMI8658.h new file mode 100644 index 0000000..ebda04b --- /dev/null +++ b/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 +#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 +*/ +/// +#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 + +/// +#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 diff --git a/QMI8658reg.h b/QMI8658reg.h new file mode 100644 index 0000000..9b2cdb4 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/README.md b/README.md new file mode 100644 index 0000000..ec5aae4 --- /dev/null +++ b/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 diff --git a/RoArm-M2_module.h b/RoArm-M2_module.h new file mode 100644 index 0000000..a6b990c --- /dev/null +++ b/RoArm-M2_module.h @@ -0,0 +1,1303 @@ +#define ANG2DEG 0.017453292 + +// Instantiate a servo control object. +SMS_STS st; + +// place holder. +void serialCtrl(); + +// Used to store the feedback information from the servo. +struct ServoFeedback { + bool status; + int pos; + int speed; + int load; + float voltage; + float current; + float temper; + byte mode; +}; + +ServoFeedback servoFeedback[5]; +// [0] BASE_SERVO_ID +// [1] SHOULDER_DRIVING_SERVO_ID +// [2] SHOULDER_DRIVEN_SERVO_ID +// [3] ELBOW_SERVO_ID +// [4] GRIPPER_SERVO_ID + + + +// input the angle in radians, and it returns the number of servo steps. +double calculatePosByRad(double radInput) { + return round((radInput / (2 * M_PI)) * ARM_SERVO_POS_RANGE); +} + +double ang2deg(double inputAng) { + return (inputAng / 180) * M_PI; +} + +// input the number of servo steps and the joint name +// return the joint angle in radians. +double calculateRadByFeedback(int inputSteps, int jointName) { + double getRad; + switch(jointName){ + case BASE_JOINT: + getRad = -(inputSteps * 2 * M_PI / ARM_SERVO_POS_RANGE) + M_PI; + break; + case SHOULDER_JOINT: + getRad = (inputSteps * 2 * M_PI / ARM_SERVO_POS_RANGE) - M_PI; + break; + case ELBOW_JOINT: + getRad = (inputSteps * 2 * M_PI / ARM_SERVO_POS_RANGE) - (M_PI / 2); + break; + case EOAT_JOINT: + getRad = inputSteps * 2 * M_PI / ARM_SERVO_POS_RANGE; + break; + } + return getRad; +} + + +// input the ID of the servo, +// and get the information saved in servoFeedback[5]. +// returnType: false - return everything. +// true - return only when failed. +bool getFeedback(byte servoID, bool returnType) { + if(st.FeedBack(servoID)!=-1) { + servoFeedback[servoID - 11].status = true; + servoFeedback[servoID - 11].pos = st.ReadPos(-1); + servoFeedback[servoID - 11].speed = st.ReadSpeed(-1); + servoFeedback[servoID - 11].load = st.ReadLoad(-1); + servoFeedback[servoID - 11].voltage = st.ReadVoltage(-1); + servoFeedback[servoID - 11].current = st.ReadCurrent(-1); + servoFeedback[servoID - 11].temper = st.ReadTemper(-1); + servoFeedback[servoID - 11].mode = st.ReadMode(servoID); + if(!returnType){ + if(InfoPrint == 1){ + jsonInfoHttp.clear(); + jsonInfoHttp["T"] = 1005; + jsonInfoHttp["id"] = servoID; + jsonInfoHttp["status"] = 1; + String getInfoJsonString; + serializeJson(jsonInfoHttp, getInfoJsonString); + Serial.println(getInfoJsonString); + } + } + else{ + return true; + } + return true; + } else{ + servoFeedback[servoID - 11].status = false; + if(InfoPrint == 1){ + jsonInfoHttp.clear(); + jsonInfoHttp["T"] = 1005; + jsonInfoHttp["id"] = servoID; + jsonInfoHttp["status"] = 0; + String getInfoJsonString; + serializeJson(jsonInfoHttp, getInfoJsonString); + Serial.println(getInfoJsonString); + } + return false; + } +} + + +// input the old servo ID and the new ID you want it to change to. +void changeID(byte oldID, byte newID) { + if(oldID == 254){ + st.unLockEprom(oldID); + st.writeByte(oldID, SMS_STS_ID, newID); + st.LockEprom(newID); + + if(InfoPrint == 1) {Serial.print("change: ");Serial.print(oldID);Serial.println(" succeed");} + return; + } + if(!getFeedback(oldID, true)) { + if(InfoPrint == 1) {Serial.print("change: ");Serial.print(oldID);Serial.println(" failed");} + return; + } + else { + st.unLockEprom(oldID); + st.writeByte(oldID, SMS_STS_ID, newID); + st.LockEprom(newID); + + if(InfoPrint == 1) {Serial.print("change: ");Serial.print(oldID);Serial.println(" succeed");} + return; + } +} + + +// ctrl the torque lock of a servo. +// input the servo ID and command: 1-on : produce torque. +// 0-off: release torque. +void servoTorqueCtrl(byte servoID, u8 enableCMD){ + st.EnableTorque(servoID, enableCMD); +} + + +// set the current position as the middle position of the servo. +// input the ID of the servo that you wannna set middle position. +void setMiddlePos(byte InputID){ + st.CalibrationOfs(InputID); +} + + +// to release all servos' torque for 10s. +void emergencyStopProcessing() { + st.EnableTorque(254, 0); + +} + + +// position check. +// it will wait for the servo to move to the goal position. +void waitMove2Goal(byte InputID, s16 goalPosition, s16 offSet){ + while(servoFeedback[InputID - 11].pos < goalPosition - offSet || + servoFeedback[InputID - 11].pos > goalPosition + offSet){ + if (!servoFeedback[InputID - 11].status) { + servoTorqueCtrl(254, 0); + break; + } + getFeedback(InputID, true); + delay(10); + } +} + + +// initialize bus servo libraris and uart2ttl. +void RoArmM2_servoInit(){ + Serial1.begin(1000000, SERIAL_8N1, S_RXD, S_TXD); + st.pSerial = &Serial1; + while(!Serial1) {} + if(InfoPrint == 1){Serial.println("ServoCtrl init succeed.");} +} + + +// check the status of every servo, +// if all status are ok, set the RoArmM2_initCheckSucceed as 1. +// 0: used to init check, print everything. +// 1: used to check while working, print when failed. +void RoArmM2_initCheck(bool returnType) { + RoArmM2_initCheckSucceed = false; + RoArmM2_initCheckSucceed = getFeedback(BASE_SERVO_ID, true) && + getFeedback(SHOULDER_DRIVING_SERVO_ID, true) && + getFeedback(SHOULDER_DRIVEN_SERVO_ID, true) && + getFeedback(ELBOW_SERVO_ID, true); + if(!returnType){ + if(InfoPrint == 1 || RoArmM2_initCheckSucceed){Serial.println("All bus servos status checked.");} + else if(InfoPrint == 1 || !RoArmM2_initCheckSucceed){Serial.println("Bus servos status check: failed.");} + } + else if(returnType && RoArmM2_initCheckSucceed){} + else if(returnType && !RoArmM2_initCheckSucceed){ + if(InfoPrint == 1){Serial.println("Check failed.");} + } +} + + +// set all servos PID as the RoArm-M2 settings. +bool setServosPID(byte InputID, byte InputP) { + if(!getFeedback(InputID, true)){return false;} + st.unLockEprom(InputID); + st.writeByte(InputID, ST_PID_P_ADDR, InputP); + st.LockEprom(InputID); + return true; +} + + +// move every joint to its init position. +// it moves only when RoArmM2_initCheckSucceed is 1. +void RoArmM2_moveInit() { + if(!RoArmM2_initCheckSucceed){ + if(InfoPrint == 1){Serial.println("Init failed, skip moveInit.");} + return; + } + else if(InfoPrint == 1){Serial.println("Stop moving to initPos.");} + + // move BASE_SERVO to middle position. + if(InfoPrint == 1){Serial.println("Moving BASE_JOINT to initPos.");} + st.WritePosEx(BASE_SERVO_ID, ARM_SERVO_MIDDLE_POS, ARM_SERVO_INIT_SPEED, ARM_SERVO_INIT_ACC); + + // release SHOULDER_DRIVEN_SERVO torque. + if(InfoPrint == 1){Serial.println("Unlock the torque of SHOULDER_DRIVEN_SERVO.");} + servoTorqueCtrl(SHOULDER_DRIVEN_SERVO_ID, 0); + + // move SHOULDER_DRIVING_SERVO to middle position. + if(InfoPrint == 1){Serial.println("Moving SHOULDER_JOINT to initPos.");} + st.WritePosEx(SHOULDER_DRIVING_SERVO_ID, ARM_SERVO_MIDDLE_POS, ARM_SERVO_INIT_SPEED, ARM_SERVO_INIT_ACC); + + // check SHOULDER_DRIVEING_SERVO position. + if(InfoPrint == 1){Serial.println("...");} + waitMove2Goal(SHOULDER_DRIVING_SERVO_ID, ARM_SERVO_MIDDLE_POS, 30); + + // wait for the jitter to go away. + delay(1200); + + // set the position as the middle of the SHOULDER_DRIVEN_SERVO. + if(InfoPrint == 1){Serial.println("Set this pos as the middle pos for SHOULDER_DRIVEN_SERVO.");} + setMiddlePos(SHOULDER_DRIVEN_SERVO_ID); + + // SHOULDER_DRIVEN_SERVO starts producing torque. + if(InfoPrint == 1){Serial.println("SHOULDER_DRIVEN_SERVO starts producing torque.");} + servoTorqueCtrl(SHOULDER_DRIVEN_SERVO_ID, 1); + delay(10); + + // move ELBOW_SERVO to middle position. + if(InfoPrint == 1){Serial.println("Moving ELBOW_SERVO to middle position.");} + st.WritePosEx(ELBOW_SERVO_ID, ARM_SERVO_MIDDLE_POS, ARM_SERVO_INIT_SPEED, ARM_SERVO_INIT_ACC); + waitMove2Goal(ELBOW_SERVO_ID, ARM_SERVO_MIDDLE_POS, 20); + + if(InfoPrint == 1){Serial.println("Moving GRIPPER_SERVO to middle position.");} + st.WritePosEx(GRIPPER_SERVO_ID, ARM_SERVO_MIDDLE_POS, ARM_SERVO_INIT_SPEED, ARM_SERVO_INIT_ACC); + + delay(1000); +} + + +// // // single joint ctrl for simple uses, base on radInput // // // + +// use this function to compute the servo position to ctrl base joint. +// returnType 0: only returns the base joint servo position and save it to goalPos[0], +// servo will NOT move. +// 1: returns the base joint servo position and save it to goalPos[0], +// servo moves. +// input the angle in radius(double), the speedInput(u16) is servo steps/second, +// the accInput(u8) is the acceleration of the servo movement. +// radInput increase, move to left. +int RoArmM2_baseJointCtrlRad(byte returnType, double radInput, u16 speedInput, u8 accInput) { + radInput = -constrain(radInput, -M_PI, M_PI); + s16 computePos = calculatePosByRad(radInput) + ARM_SERVO_MIDDLE_POS; + goalPos[0] = computePos; + + if(returnType){ + st.WritePosEx(BASE_SERVO_ID, goalPos[0], speedInput, accInput); + } + return goalPos[0]; +} + + +// use this function to compute the servo position to ctrl shoudlder joint. +// returnType 0: only returns the shoulder joint servo position and save it to goalPos[1] and goalPos[2], +// servo will NOT move. +// 1: returns the shoulder joint servo position and save it to goalPos[1] and goalPos[2], +// servo moves. +// input the angle in radius(double), the speedInput(u16) is servo steps/second, +// the accInput(u8) is the acceleration of the servo movement. +// radInput increase, it leans forward. +int RoArmM2_shoulderJointCtrlRad(byte returnType, double radInput, u16 speedInput, u8 accInput) { + radInput = constrain(radInput, -M_PI/2, M_PI/2); + s16 computePos = calculatePosByRad(radInput); + goalPos[1] = ARM_SERVO_MIDDLE_POS + computePos; + goalPos[2] = ARM_SERVO_MIDDLE_POS - computePos; + + if(returnType == 1){ + st.WritePosEx(SHOULDER_DRIVING_SERVO_ID, goalPos[1], speedInput, accInput); + st.WritePosEx(SHOULDER_DRIVEN_SERVO_ID, goalPos[2], speedInput, accInput); + } + else if(returnType == SHOULDER_DRIVING_SERVO_ID){ + return goalPos[1]; + } + else if(returnType == SHOULDER_DRIVEN_SERVO_ID){ + return goalPos[2]; + } +} + + +// use this function to compute the servo position to ctrl elbow joint. +// returnType 0: only returns the elbow joint servo position and save it to goalPos[3], +// servo will NOT move. +// 1: returns the elbow joint servo position and save it to goalPos[3], +// servo moves. +// input the angle in radius(double), the speedInput(u16) is servo steps/second, +// the accInput(u8) is the acceleration of the servo movement. +// angleInput increase, it moves down. +int RoArmM2_elbowJointCtrlRad(byte returnType, double radInput, u16 speedInput, u8 accInput) { + s16 computePos = calculatePosByRad(radInput) + 1024; + goalPos[3] = constrain(computePos, 512, 3071); + + if(returnType){ + st.WritePosEx(ELBOW_SERVO_ID, goalPos[3], speedInput, accInput); + } + return goalPos[3]; +} + + +// use this function to compute the servo position to ctrl grab/hand joint. +// returnType 0: only returns the hand joint servo position and save it to goalPos[4], +// servo will NOT move. +// 1: returns the hand joint servo position and save it to goalPos[4], +// servo moves. +// ctrl type 0: status ctrl. - cmd 0: release +// 1: grab +// 1: position ctrl. - cmd: input angle in radius. +int RoArmM2_handJointCtrlRad(byte returnType, double radInput, u16 speedInput, u8 accInput) { + s16 computePos = calculatePosByRad(radInput); + goalPos[4] = constrain(computePos, 700, 3396); + + if (returnType) { + st.WritePosEx(GRIPPER_SERVO_ID, goalPos[4], speedInput, accInput); + } + return goalPos[4]; +} + + +// use this function to ctrl the max torque of base joint. +void RoArmM2_baseTorqueCtrl(int inputTorque) { + st.unLockEprom(BASE_SERVO_ID); + st.writeWord(BASE_SERVO_ID, SMS_STS_TORQUE_LIMIT_L, constrain(inputTorque, ST_TORQUE_MIN, ST_TORQUE_MAX)); + st.LockEprom(BASE_SERVO_ID); +} + + +// use this function to ctrl the max torque of shoulder joint. +void RoArmM2_shoulderTorqueCtrl(int inputTorque) { + st.unLockEprom(SHOULDER_DRIVING_SERVO_ID); + st.writeWord(SHOULDER_DRIVING_SERVO_ID, SMS_STS_TORQUE_LIMIT_L, constrain(inputTorque, ST_TORQUE_MIN, ST_TORQUE_MAX)); + st.LockEprom(SHOULDER_DRIVING_SERVO_ID); + + st.unLockEprom(SHOULDER_DRIVEN_SERVO_ID); + st.writeWord(SHOULDER_DRIVEN_SERVO_ID, SMS_STS_TORQUE_LIMIT_L, constrain(inputTorque, ST_TORQUE_MIN, ST_TORQUE_MAX)); + st.LockEprom(SHOULDER_DRIVEN_SERVO_ID); +} + + +// use this function to ctrl the max torque of elbow joint. +void RoArmM2_elbowTorqueCtrl(int inputTorque) { + st.unLockEprom(ELBOW_SERVO_ID); + st.writeWord(ELBOW_SERVO_ID, SMS_STS_TORQUE_LIMIT_L, constrain(inputTorque, ST_TORQUE_MIN, ST_TORQUE_MAX)); + st.LockEprom(ELBOW_SERVO_ID); +} + + +// use this function to ctrl the max torque of hand joint. +void RoArmM2_handTorqueCtrl(int inputTorque) { + st.unLockEprom(GRIPPER_SERVO_ID); + st.writeWord(GRIPPER_SERVO_ID, SMS_STS_TORQUE_LIMIT_L, constrain(inputTorque, ST_TORQUE_MIN, ST_TORQUE_MAX)); + st.LockEprom(GRIPPER_SERVO_ID); +} + + +// 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":50,"s":50,"e":50,"h":50} +// stop +// {"T":112,"mode":0,"b":1000,"s":1000,"e":1000,"h":1000} +void RoArmM2_dynamicAdaptation(byte inputM, int inputB, int inputS, int inputE, int inputH) { + if (inputM == 0) { + RoArmM2_baseTorqueCtrl(ST_TORQUE_MAX); + RoArmM2_shoulderTorqueCtrl(ST_TORQUE_MAX); + RoArmM2_elbowTorqueCtrl(ST_TORQUE_MAX); + RoArmM2_handTorqueCtrl(ST_TORQUE_MAX); + } else if (inputM == 1) { + RoArmM2_baseTorqueCtrl(inputB); + RoArmM2_shoulderTorqueCtrl(inputS); + RoArmM2_elbowTorqueCtrl(inputE); + RoArmM2_handTorqueCtrl(inputH); + } +} + + +// this function uses relative radInput to set a new X+ axis. +// dirInput: +// 0 +// X+ +// -90 - ^ - 90 +// | +// -180 180 +void setNewAxisX(double angleInput) { + double radInput = (angleInput / 180) * M_PI; + RoArmM2_shoulderJointCtrlRad(1, 0, 500, 20); + waitMove2Goal(SHOULDER_DRIVING_SERVO_ID, goalPos[1], 20); + + RoArmM2_elbowJointCtrlRad(1, 0, 500, 20); + waitMove2Goal(ELBOW_SERVO_ID, goalPos[3], 20); + + RoArmM2_baseJointCtrlRad(1, 0, 500, 20); + waitMove2Goal(BASE_SERVO_ID, goalPos[0], 20); + + delay(1000); + + RoArmM2_baseJointCtrlRad(1, -radInput, 500, 20); + waitMove2Goal(BASE_SERVO_ID, goalPos[0], 20); + + delay(1000); + + setMiddlePos(BASE_SERVO_ID); + + delay(5); +} + + +// Simple Linkage IK: +// input the position of the end and return angle. +// O----O +// / +// O +// --------------------------------------------------- +// | /beta /delta | +// O----LB---------X------ | +// | / omega. | \LB | +// LA . < ----------------| +// |alpha . bIn \LB -EP 0 ; delta <= 0 ; aIn, bIn > 0 +void simpleLinkageIkRad(double LA, double LB, double aIn, double bIn) { + double psi, alpha, omega, beta, L2C, LC, lambda, delta; + + if (fabs(bIn) < 1e-6) { + psi = acos((LA * LA + aIn * aIn - LB * LB) / (2 * LA * aIn)) + t2rad; + alpha = M_PI / 2.0 - psi; + omega = acos((aIn * aIn + LB * LB - LA * LA) / (2 * aIn * LB)); + beta = psi + omega - t3rad; + } else { + L2C = aIn * aIn + bIn * bIn; + LC = sqrt(L2C); + lambda = atan2(bIn, aIn); + psi = acos((LA * LA + L2C - LB * LB) / (2 * LA * LC)) + t2rad; + alpha = M_PI / 2.0 - lambda - psi; + omega = acos((LB * LB + L2C - LA * LA) / (2 * LC * LB)); + beta = psi + omega - t3rad; + } + + delta = M_PI / 2.0 - alpha - beta; + + SHOULDER_JOINT_RAD = alpha; + ELBOW_JOINT_RAD = beta; + EOAT_JOINT_RAD_BUFFER = delta; + + nanIK = isnan(alpha) || isnan(beta) || isnan(delta); +} + + +// AI prompt: +// *** this function is written with AI. *** +// ''' +// 我需要一个C语言函数，在一个平面直角坐标系中，输入一个坐标点(x,y)，返回值有两个： +// 1. 这个坐标点距离坐标系原点的距离。 +// 2. 这个点与坐标系原点所连线段与x轴正方向的夹角，夹角范围在(-PI, PI)之间。 +// ''' + +// AI prompt: +// I need a C language function. In a 2D Cartesian coordinate system, +// input a coordinate point (x, y). The function should return two values: + +// The distance from this coordinate point to the origin of the coordinate system. +// The angle, in radians, between the line connecting this point and the origin +// of the coordinate system and the positive direction of the x-axis. +// The angle should be in the range (-π, π). +void cartesian_to_polar(double x, double y, double* r, double* theta) { + *r = sqrt(x * x + y * y); + *theta = atan2(y, x); +} + + +// AI prompt: +// *** this function is written with AI. *** +// 我现在需要一个功能与上面函数相反的函数： +// 输入机械臂三个关节的轴的角度（弧度制），返回当前机械臂末端点的坐标点。 + +// 你在回答的过程中可以告诉我你还有什么其它需要的信息。 +// ''' +// use this two functions to compute the position of coordinate point +// by inputing the jointRad. +// 这个函数用于将极坐标转换为直角坐标 +void polarToCartesian(double r, double theta, double &x, double &y) { + x = r * cos(theta); + y = r * sin(theta); +} + + +// this function is used to compute the position of the end point. +// input the angle of every joint in radius. +// compute the positon and save it to lastXYZ by default. +void RoArmM2_computePosbyJointRad(double base_joint_rad, double shoulder_joint_rad, double elbow_joint_rad, double hand_joint_rad) { + if (EEMode == 0) { + // the end of the arm. + double r_ee, x_ee, y_ee, z_ee; + + // compute the end position of the first linkage(the linkage between baseJoint and shoulderJoint). + double aOut, bOut, cOut, dOut, eOut, fOut; + + polarToCartesian(l2, ((M_PI / 2) - (shoulder_joint_rad + t2rad)), aOut, bOut); + polarToCartesian(l3, ((M_PI / 2) - (elbow_joint_rad + shoulder_joint_rad)), cOut, dOut); + + r_ee = aOut + cOut; + z_ee = bOut + dOut; + + polarToCartesian(r_ee, base_joint_rad, eOut, fOut); + x_ee = eOut; + y_ee = fOut; + + lastX = x_ee; + lastY = y_ee; + lastZ = z_ee; + } + else if (EEMode == 1) { + double aOut, bOut, cOut, dOut, eOut, fOut, gOut, hOut; + double r_ee, z_ee; + + polarToCartesian(l2, ((M_PI / 2) - (shoulder_joint_rad + t2rad)), aOut, bOut); + polarToCartesian(l3, ((M_PI / 2) - (elbow_joint_rad + shoulder_joint_rad + t3rad)), cOut, dOut); + polarToCartesian(lE, -((hand_joint_rad + tErad) - M_PI - (M_PI/2 - shoulder_joint_rad - elbow_joint_rad)), eOut, fOut); + + r_ee = aOut + cOut + eOut; + z_ee = bOut + dOut + fOut; + + polarToCartesian(r_ee, base_joint_rad, gOut, hOut); + + lastX = gOut; + lastY = hOut; + lastZ = z_ee; + lastT = hand_joint_rad - (M_PI - shoulder_joint_rad - elbow_joint_rad) + (M_PI / 2); + } +} + + +// EEmode funcs change here. +// get position by servo feedback. +void RoArmM2_getPosByServoFeedback() { + getFeedback(BASE_SERVO_ID, true); + getFeedback(SHOULDER_DRIVING_SERVO_ID, true); + getFeedback(ELBOW_SERVO_ID, true); + getFeedback(GRIPPER_SERVO_ID, true); + + radB = calculateRadByFeedback(servoFeedback[BASE_SERVO_ID - 11].pos, BASE_JOINT); + radS = calculateRadByFeedback(servoFeedback[SHOULDER_DRIVING_SERVO_ID - 11].pos, SHOULDER_JOINT); + radE = calculateRadByFeedback(servoFeedback[ELBOW_SERVO_ID - 11].pos, ELBOW_JOINT); + radG = calculateRadByFeedback(servoFeedback[GRIPPER_SERVO_ID - 11].pos, EOAT_JOINT); + + RoArmM2_computePosbyJointRad(radB, radS, radE, radG); + if (EEMode == 0) { + lastT = radG; + } +} + + +// feedback info in json. +void RoArmM2_infoFeedback() { + jsonInfoHttp.clear(); + jsonInfoHttp["T"] = 1051; + jsonInfoHttp["x"] = lastX; + jsonInfoHttp["y"] = lastY; + jsonInfoHttp["z"] = lastZ; + jsonInfoHttp["b"] = radB; + jsonInfoHttp["s"] = radS; + jsonInfoHttp["e"] = radE; + jsonInfoHttp["t"] = lastT; + // jsonInfoHttp["goalX"] = goalX; + // jsonInfoHttp["goalY"] = goalY; + // jsonInfoHttp["goalZ"] = goalZ; + // jsonInfoHttp["goalT"] = goalT; + 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; + + String getInfoJsonString; + serializeJson(jsonInfoHttp, getInfoJsonString); + Serial.println(getInfoJsonString); +} + + +// AI prompt: +// 在平面直角坐标系中，有一点A，输入点A的X,Y坐标和角度参数theta(弧度制)，点A绕直角坐标系原点 +// 逆时针转动theta作为点B，返回点B的XY坐标值，我需要C语言的函数。 +// AI prompt: +// In a 2D Cartesian coordinate system, there is a point A. +// Input the X and Y coordinates of point A and an angle parameter theta (in radians). +// Point A rotates counterclockwise around the origin of the Cartesian coordinate +// system by an angle of theta to become point B. Return the XY coordinates of point B. +// I need a C language function. +void rotatePoint(double theta, double *xB, double *yB) { + double alpha = tErad + theta; + + *xB = lE * cos(alpha); + *yB = lE * sin(alpha); +} + + +// AI prompt: +// 在平面直角坐标系种，有一点A，输入点A的X,Y坐标值，输入一个距离参数S， +// 点A向原点方向移动S作为点B，返回点B的坐标值。我需要C语言的函数。 +void movePoint(double xA, double yA, double s, double *xB, double *yB) { + double distance = sqrt(pow(xA, 2) + pow(yA, 2)); + if(distance - s <= 1e-6) { + *xB = 0; + *yB = 0; + } + else { + double ratio = (distance - s) / distance; + *xB = xA * ratio; + *yB = yA * ratio; + } +} + + +// ---===< Muti-assembly IK config here >===--- +// change this func and goalPosMove() +// Coordinate Ctrl: input the coordinate point of the goal position to compute +// the goalPos of every joints. +void RoArmM2_baseCoordinateCtrl(double inputX, double inputY, double inputZ, double inputT){ + if (EEMode == 0) { + cartesian_to_polar(inputX, inputY, &base_r, &BASE_JOINT_RAD); + simpleLinkageIkRad(l2, l3, base_r, inputZ); + RoArmM2_handJointCtrlRad(0, inputT, 0, 0); + } + else if (EEMode == 1) { + rotatePoint((inputT - M_PI), &delta_x, &delta_y); + movePoint(inputX, inputY, delta_x, &beta_x, &beta_y); + cartesian_to_polar(beta_x, beta_y, &base_r, &BASE_JOINT_RAD); + simpleLinkageIkRad(l2, l3, base_r, inputZ + delta_y); + EOAT_JOINT_RAD = EOAT_JOINT_RAD_BUFFER + inputT; + } +} + + +// update last position for later use. +void RoArmM2_lastPosUpdate(){ + lastX = goalX; + lastY = goalY; + lastZ = goalZ; + lastT = goalT; +} + + +// use jointCtrlRad functions to compute goalPos for every servo, +// then use this function to move the servos. +// cuz the functions like baseCoordinateCtrl is not gonna make servos move. +void RoArmM2_goalPosMove(){ + RoArmM2_baseJointCtrlRad(0, BASE_JOINT_RAD, 0, 0); + RoArmM2_shoulderJointCtrlRad(0, SHOULDER_JOINT_RAD, 0, 0); + RoArmM2_elbowJointCtrlRad(0, ELBOW_JOINT_RAD, 0, 0); + if (EEMode == 1) { + RoArmM2_handJointCtrlRad(0, EOAT_JOINT_RAD, 0, 0); + } + st.SyncWritePosEx(servoID, 5, goalPos, moveSpd, moveAcc); +} + + +void RoArmM2_uiCtrl(float inputE, float inputZ, float inputR) { + simpleLinkageIkRad(l2, l3, inputE, inputZ); + BASE_JOINT_RAD = ang2deg(inputR); + RoArmM2_goalPosMove(); +} + + +// ctrl a single joint abs angle(rad). +// joint: 1-BASE_JOINT + ->left +// 2-SHOULDER_JOINT + ->down +// 3-ELBOW_JOINT + ->down +// 4-EOAT_JOINT + ->grab/down +// inputRad: input the goal angle in radius of the joint. +// inputSpd: move speed, steps/second. +// inputAcc: acceleration, steps/second^2. +void RoArmM2_singleJointAbsCtrl(byte jointInput, double inputRad, u16 inputSpd, u8 inputAcc){ + switch(jointInput){ + case BASE_JOINT: + RoArmM2_baseJointCtrlRad(1, inputRad, inputSpd, inputAcc); + BASE_JOINT_RAD = inputRad; + break; + case SHOULDER_JOINT: + RoArmM2_shoulderJointCtrlRad(1, inputRad, inputSpd, inputAcc); + SHOULDER_JOINT_RAD = inputRad; + break; + case ELBOW_JOINT: + RoArmM2_elbowJointCtrlRad(1, inputRad, inputSpd, inputAcc); + ELBOW_JOINT_RAD = inputRad; + break; + case EOAT_JOINT: + RoArmM2_handJointCtrlRad(1, inputRad, inputSpd, inputAcc); + EOAT_JOINT_RAD = inputRad; + break; + } + RoArmM2_computePosbyJointRad(BASE_JOINT_RAD, SHOULDER_JOINT_RAD, ELBOW_JOINT_RAD, EOAT_JOINT_RAD); +} + + +// ctrl all joints together. +// when all joints in initPos(middle position), it looks like below. +// -------L3------------O==L2B=== +// ^ | +// | | +// ELBOW_JOINT | +// L2A +// | +// | +// ^ | +// | SHOULDER_JOINT -> O +// Z+ | +// | L1 +// | | +// <---X+--Y+ BASE_JOINT -> X +// +// +// -------L3------------O==L2B==O <- BASE_JOINT +// ^ +// <---X+--Z+ | +// | ELBOW_JOINT +// Y+ +// | +// v +void RoArmM2_allJointAbsCtrl(double inputBase, double inputShoulder, double inputElbow, double inputHand, u16 inputSpd, u8 inputAcc){ + RoArmM2_baseJointCtrlRad(0, inputBase, inputSpd, inputAcc); + RoArmM2_shoulderJointCtrlRad(0, inputShoulder, inputSpd, inputAcc); + RoArmM2_elbowJointCtrlRad(0, inputElbow, inputSpd, inputAcc); + RoArmM2_handJointCtrlRad(0, inputHand, inputSpd, inputAcc); + for (int i = 0;i < 5;i++) { + moveSpd[i] = inputSpd; + moveAcc[i] = inputAcc; + } + st.SyncWritePosEx(servoID, 5, goalPos, moveSpd, moveAcc); + for (int i = 0;i < 5;i++) { + moveSpd[i] = 0; + moveAcc[i] = 0; + } +} + + +// ctrl the movement in a smooth way. +// | .. <-numEnd +// | . | +// | . +// | . | +// | . +// | . | +// |. . <-numStart +// ---------------------- +// 0 1 rateInput +double besselCtrl(double numStart, double numEnd, double rateInput){ + double numOut; + numOut = (numEnd - numStart)*((cos(rateInput*M_PI+M_PI)+1)/2) + numStart; + return numOut; +} + + +// use this function to get the max deltaSteps. +// get the max offset between [goal] and [last] position. +double maxNumInArray(){ + if (EEMode == 0) { + double deltaPos[4] = {abs(goalX - lastX), + abs(goalY - lastY), + abs(goalZ - lastZ), + abs(goalT - lastT)*10}; + double maxVal = deltaPos[0]; + for(int i = 0; i < (sizeof(deltaPos) / sizeof(deltaPos[0])); i++){ + maxVal = max(deltaPos[i],maxVal); + } + return maxVal; + } else if (EEMode == 1) { + double deltaPos[4] = {abs(goalX - lastX), + abs(goalY - lastY), + abs(goalZ - lastZ), + abs(goalT - lastT)*200}; + double maxVal = deltaPos[0]; + for(int i = 0; i < (sizeof(deltaPos) / sizeof(deltaPos[0])); i++){ + maxVal = max(deltaPos[i],maxVal); + } + return maxVal; + } +} + + +// use this function to move the end of the arm to the goal position. +void RoArmM2_movePosGoalfromLast(float spdInput){ + double deltaSteps = maxNumInArray(); + + double bufferX; + double bufferY; + double bufferZ; + double bufferT; + + static double bufferLastX; + static double bufferLastY; + static double bufferLastZ; + static double bufferLastT; + + for(double i=0;i<=1;i+=(1/(deltaSteps*1))*spdInput){ + bufferX = besselCtrl(lastX, goalX, i); + bufferY = besselCtrl(lastY, goalY, i); + bufferZ = besselCtrl(lastZ, goalZ, i); + bufferT = besselCtrl(lastT, goalT, i); + RoArmM2_baseCoordinateCtrl(bufferX, bufferY, bufferZ, bufferT); + if(nanIK){ + // IK failed + goalX = bufferLastX; + goalY = bufferLastY; + goalZ = bufferLastZ; + goalT = bufferLastT; + RoArmM2_baseCoordinateCtrl(goalX, goalY, goalZ, goalT); + RoArmM2_goalPosMove(); + RoArmM2_lastPosUpdate(); + return; + } + else{ + // IK succeed. + bufferLastX = bufferX; + bufferLastY = bufferY; + bufferLastZ = bufferZ; + bufferLastT = bufferT; + } + RoArmM2_goalPosMove(); + delay(2); + } + RoArmM2_baseCoordinateCtrl(goalX, goalY, goalZ, goalT); + RoArmM2_goalPosMove(); + RoArmM2_lastPosUpdate(); +} + + +// ctrl a single axi abs pos(mm). +// the init position is +// axiInput: 1-X, posInput:initX +// 2-Y, posInput:initY +// 3-Z, posInput:initZ +// 4-T, posInput:initT +// initX = l3+l2B +// initY = 0 +// initZ = l2A +// initT = M_PI +// default inputSpd = 0.25 +void RoArmM2_singlePosAbsBesselCtrl(byte axiInput, double posInput, double inputSpd){ + switch(axiInput){ + case 1: goalX = posInput;break; + case 2: goalY = posInput;break; + case 3: goalZ = posInput;break; + case 4: goalT = posInput;break; + } + RoArmM2_movePosGoalfromLast(inputSpd); +} + + +// ctrl all axis abs position. +// initX = l3+l2B +// initY = 0 +// initZ = l2A +// initT = M_PI +// default inputSpd = 0.36 +void RoArmM2_allPosAbsBesselCtrl(double inputX, double inputY, double inputZ, double inputT, double inputSpd){ + goalX = inputX; + goalY = inputY; + goalZ = inputZ; + goalT = inputT; + RoArmM2_movePosGoalfromLast(inputSpd); +} + + +// ChatGPT prompt: +// ''' +// 我需要一个函数，输入圆心坐标点、半径和比例，当比例从0到1变化时，函数输出的坐标点可以组成一个完整的圆。 +// ''' +// I need a function that inputs the center coordinate point, +// radius and scale(t), and when the scale(t) changes from 0 to 1, +// the coordinate points output by the function can form a complete circle. +// ''' +// +// example: +// for(float i=0;i<=1;i+=0.001){ +// getCirclePointYZ(0, initZ, 100, i); +// RoArmM2_goalPosMove(); +// delay(5); +// } +void getCirclePointYZ(double cx, double cy, double r, double t) { + double theta = t * 2 * M_PI; + goalY = cx + r * cos(theta); + goalZ = cy + r * sin(theta); +} + + +// delay cmd. +void RoArmM2_delayMillis(int inputTime) { + delay(inputTime); +} + + +// set the P&I/PID of a joint. +void RoArmM2_setJointPID(byte jointInput, float inputP, float inputI) { + switch (jointInput) { + case BASE_JOINT: + st.writeByte(BASE_SERVO_ID, ST_PID_P_ADDR, inputP); + st.writeByte(BASE_SERVO_ID, ST_PID_I_ADDR, inputI); + break; + case SHOULDER_JOINT: + st.writeByte(SHOULDER_DRIVING_SERVO_ID, ST_PID_P_ADDR, inputP); + st.writeByte(SHOULDER_DRIVING_SERVO_ID, ST_PID_I_ADDR, inputI); + + st.writeByte(SHOULDER_DRIVEN_SERVO_ID, ST_PID_P_ADDR, inputP); + st.writeByte(SHOULDER_DRIVEN_SERVO_ID, ST_PID_I_ADDR, inputI); + break; + case ELBOW_JOINT: + st.writeByte(ELBOW_SERVO_ID, ST_PID_P_ADDR, inputP); + st.writeByte(ELBOW_SERVO_ID, ST_PID_I_ADDR, inputI); + break; + case EOAT_JOINT: + st.writeByte(GRIPPER_SERVO_ID, ST_PID_P_ADDR, inputP); + st.writeByte(GRIPPER_SERVO_ID, ST_PID_I_ADDR, inputI); + break; + } +} + + +// reset the P&I/PID of RoArm-M2. +void RoArmM2_resetPID() { + RoArmM2_setJointPID(BASE_JOINT, 16, 0); + RoArmM2_setJointPID(SHOULDER_JOINT, 16, 0); + RoArmM2_setJointPID(ELBOW_JOINT, 16, 0); + RoArmM2_setJointPID(EOAT_JOINT, 16, 0); +} + + +// input the angle in deg, and it returns the number of servo steps. +int calculatePosByDeg(double degInput) { + return round((degInput / 360) * ARM_SERVO_POS_RANGE); +} + + +// ctrl a single joint abs angle. +// jointInput: 1-BASE_JOINT +// 2-SHOULDER_JOINT +// 3-ELBOW_JOINT +// 4-HAND_JOINT +// inputRad: input the goal angle in deg of the joint. +// inputSpd: move speed, angle/second. +// inputAcc: acceleration, angle/second^2. +void RoArmM2_singleJointAngleCtrl(byte jointInput, double inputAng, u16 inputSpd, u8 inputAcc){ + Serial.println("---"); + Serial.print(jointInput);Serial.print("\t");Serial.print(inputAng);Serial.print("\t"); + Serial.print(inputSpd);Serial.print("\t");Serial.print(inputAcc);Serial.println(); + + inputSpd = abs(inputSpd); + inputAcc = abs(inputAcc); + switch(jointInput){ + case BASE_JOINT: + BASE_JOINT_ANG = inputAng; + Serial.println(inputAng); + BASE_JOINT_RAD = ang2deg(inputAng); + Serial.println(BASE_JOINT_RAD); + RoArmM2_baseJointCtrlRad(1, BASE_JOINT_RAD, calculatePosByDeg(inputSpd), calculatePosByDeg(inputAcc)); + break; + case SHOULDER_JOINT: + SHOULDER_JOINT_ANG = inputAng; + SHOULDER_JOINT_RAD = ang2deg(inputAng); + RoArmM2_shoulderJointCtrlRad(1, SHOULDER_JOINT_RAD, calculatePosByDeg(inputSpd), calculatePosByDeg(inputAcc)); + break; + case ELBOW_JOINT: + ELBOW_JOINT_ANG = inputAng; + ELBOW_JOINT_RAD = ang2deg(inputAng); + RoArmM2_elbowJointCtrlRad(1, ELBOW_JOINT_RAD, calculatePosByDeg(inputSpd), calculatePosByDeg(inputAcc)); + break; + case EOAT_JOINT: + EOAT_JOINT_ANG = inputAng; + EOAT_JOINT_RAD = ang2deg(inputAng); + RoArmM2_handJointCtrlRad(1, EOAT_JOINT_RAD, calculatePosByDeg(inputSpd), calculatePosByDeg(inputAcc)); + break; + } + RoArmM2_computePosbyJointRad(BASE_JOINT_RAD, SHOULDER_JOINT_RAD, ELBOW_JOINT_RAD, EOAT_JOINT_RAD); +} + + +// ctrl all joints together. +// when all joints in initPos(middle position), it looks like below. +// -------L3------------O==L2B=== +// ^ | +// | | +// ELBOW_JOINT | +// L2A +// | +// | +// ^ | +// | SHOULDER_JOINT -> O +// Z+ | +// | L1 +// | | +// <---X+--Y+ BASE_JOINT -> X +// +// +// -------L3------------O==L2B==O <- BASE_JOINT +// ^ +// <---X+--Z+ | +// | ELBOW_JOINT +// Y+ +// | +// v +void RoArmM2_allJointsAngleCtrl(double inputBase, double inputShoulder, double inputElbow, double inputHand, u16 inputSpd, u8 inputAcc){ + BASE_JOINT_ANG = inputBase; + BASE_JOINT_RAD = ang2deg(inputBase); + + SHOULDER_JOINT_ANG = inputShoulder; + SHOULDER_JOINT_RAD = ang2deg(inputShoulder); + + ELBOW_JOINT_ANG = inputElbow; + ELBOW_JOINT_RAD = ang2deg(inputElbow); + + EOAT_JOINT_ANG = inputHand; + EOAT_JOINT_RAD = ang2deg(inputHand); + + RoArmM2_baseJointCtrlRad(0, BASE_JOINT_RAD, 0, 0); + RoArmM2_shoulderJointCtrlRad(0, SHOULDER_JOINT_RAD, 0, 0); + RoArmM2_elbowJointCtrlRad(0, ELBOW_JOINT_RAD, 0, 0); + RoArmM2_handJointCtrlRad(0, EOAT_JOINT_RAD, 0, 0); + inputSpd = abs(calculatePosByDeg(inputSpd)); + inputAcc = abs(calculatePosByDeg(inputAcc)); + for (int i = 0;i < 5;i++) { + moveSpd[i] = inputSpd; + moveAcc[i] = inputAcc; + } + st.SyncWritePosEx(servoID, 5, goalPos, moveSpd, moveAcc); +} + + +void constantCtrl(byte inputMode, byte inputAxis, byte inputCmd, byte inputSpd) { + const_mode = inputMode; + if (const_mode == CONST_ANGLE) { + const_spd = abs(inputSpd) * 0.0005; + } else if (const_mode == CONST_XYZT) { + const_spd = abs(inputSpd) * 0.1; + } + + switch (inputAxis) { + case BASE_JOINT: + const_cmd_base_x = inputCmd; + break; + case SHOULDER_JOINT: + const_cmd_shoulder_y = inputCmd; + break; + case ELBOW_JOINT: + const_cmd_elbow_z = inputCmd; + break; + case EOAT_JOINT: + const_cmd_eoat_t = inputCmd; + break; + } +} + + +// RoArmM2_infoFeedback() +void constantHandle() { + if (!const_cmd_base_x && !const_cmd_shoulder_y && !const_cmd_elbow_z && !const_cmd_eoat_t) { + const_goal_base = radB; + const_goal_shoulder = radS; + const_goal_elbow = radE; + const_goal_eoat = radG; + + goalX = lastX; + goalY = lastY; + goalZ = lastZ; + goalT = lastT; + return; + } + + if (const_cmd_base_x == MOVE_INCREASE) { + if (const_mode == CONST_ANGLE) { + const_goal_base += const_spd; + if (const_goal_base > M_PI) { + const_goal_base = M_PI; + const_cmd_base_x = MOVE_STOP; + } + } + else if (const_mode == CONST_XYZT) { + goalX += const_spd; + } + } else if (const_cmd_base_x == MOVE_DECREASE) { + if (const_mode == CONST_ANGLE) { + const_goal_base -= const_spd; + if (const_goal_base < -M_PI) { + const_goal_base = -M_PI; + const_cmd_base_x = MOVE_STOP; + } + } + else if (const_mode == CONST_XYZT) { + goalX -= const_spd; + } + } + + + if (const_cmd_shoulder_y == MOVE_INCREASE) { + if (const_mode == CONST_ANGLE) { + const_goal_shoulder += const_spd; + if (const_goal_shoulder > M_PI/2) { + const_goal_shoulder = M_PI/2; + const_cmd_shoulder_y = MOVE_STOP; + } + } + else if (const_mode == CONST_XYZT) { + goalY += const_spd; + } + } else if (const_cmd_shoulder_y == MOVE_DECREASE) { + if (const_mode == CONST_ANGLE) { + const_goal_shoulder -= const_spd; + if (const_goal_shoulder < -M_PI/2) { + const_goal_shoulder = -M_PI/2; + const_cmd_shoulder_y = MOVE_STOP; + } + } + else if (const_mode == CONST_XYZT) { + goalY -= const_spd; + } + } + + + if (const_cmd_elbow_z == MOVE_INCREASE) { + if (const_mode == CONST_ANGLE) { + const_goal_elbow += const_spd; + if (const_goal_elbow > M_PI) { + const_goal_elbow = M_PI; + const_cmd_elbow_z = MOVE_STOP; + } + } + else if (const_mode == CONST_XYZT) { + goalZ += const_spd; + } + } else if (const_cmd_elbow_z == MOVE_DECREASE) { + if (const_mode == CONST_ANGLE) { + const_goal_elbow -= const_spd; + if (const_goal_elbow < -M_PI/4) { + const_goal_elbow = -M_PI/4; + const_cmd_elbow_z = MOVE_STOP; + } + } + else if (const_mode == CONST_XYZT) { + goalZ -= const_spd; + } + } + + + if (const_cmd_eoat_t == MOVE_INCREASE) { + if (const_mode == CONST_ANGLE) { + const_goal_eoat += const_spd; + if (const_goal_eoat > M_PI*7/4) { + const_goal_eoat = M_PI*7/4; + const_cmd_eoat_t = MOVE_STOP; + } + } + else if (const_mode == CONST_XYZT) { + goalT += const_spd/200; + } + } else if (const_cmd_eoat_t == MOVE_DECREASE) { + if (const_mode == CONST_ANGLE) { + const_goal_eoat -= const_spd; + if (const_goal_eoat < -M_PI/4) { + const_goal_eoat = -M_PI/4; + const_cmd_eoat_t = MOVE_STOP; + } + } + else if (const_mode == CONST_XYZT) { + goalT -= const_spd/200; + } + } + + if (const_mode == CONST_ANGLE) { + RoArmM2_allJointAbsCtrl(const_goal_base, const_goal_shoulder, const_goal_elbow, const_goal_eoat, 0, 0); + } else if (const_mode == CONST_XYZT) { + + static double bufferLastX; + static double bufferLastY; + static double bufferLastZ; + static double bufferLastT; + + RoArmM2_baseCoordinateCtrl(goalX, goalY, goalZ, goalT); + if (nanIK) { + // IK failed + goalX = bufferLastX; + goalY = bufferLastY; + goalZ = bufferLastZ; + goalT = bufferLastT; + RoArmM2_baseCoordinateCtrl(bufferLastX, bufferLastY, bufferLastZ, bufferLastT); + RoArmM2_goalPosMove(); + RoArmM2_lastPosUpdate(); + return; + } + else { + bufferLastX = goalX; + bufferLastY = goalY; + bufferLastZ = goalZ; + bufferLastT = goalT; + } + RoArmM2_goalPosMove(); + RoArmM2_lastPosUpdate(); + } +} + + + +// // // // // // // // // // // // // // // // +// // // // // // +// // // // // // // // // // // // // // // // +// example: +// RoArmM2_Test_drawSqureYZ(-100, 0, 50, 300); +// void RoArmM2_Test_drawSqureXZ(int squre_x, int squre_y, int squre_l){ +// for(double i=0;i<=squre_l;i+=0.1){ +// simpleLinkageIkRad(l2, l3, l3+l2B + squre_x, l2A-i+squre_y); +// RoArmM2_goalPosMove(); +// delay(3); +// } +// delay(1500); + +// for(double i=0;i<=squre_l;i+=0.1){ +// simpleLinkageIkRad(l2, l3, l3+l2B-i + squre_x , l2A- squre_l+squre_y); +// RoArmM2_goalPosMove(); +// delay(3); +// } +// delay(1500); + +// for(double i=0;i<=squre_l;i+=0.1){ +// simpleLinkageIkRad(l2, l3, l3+l2B- squre_l +squre_x, l2A- squre_l +i+squre_y); +// RoArmM2_goalPosMove(); +// delay(3); +// } +// delay(1500); + +// for(double i=0;i<=squre_l;i+=0.1){ +// simpleLinkageIkRad(l2, l3, l3+l2B- squre_l +i +squre_x, l2A +squre_y); +// RoArmM2_goalPosMove(); +// delay(3); +// } +// delay(1500); +// } + + +// void RoArmM2_Test_drawSqureYZ(int squre_x, int squre_y, int squre_z, int squre_l){ +// for(double i=0;i<=squre_l;i+=0.1){ +// RoArmM2_baseCoordinateCtrl(l3+l2B+squre_x, squre_y-squre_l/2+i, l2A+squre_z); +// RoArmM2_goalPosMove(); +// delay(2); +// } +// delay(1250); + +// for(double i=0;i<=squre_l;i+=0.1){ +// RoArmM2_baseCoordinateCtrl(l3+l2B+squre_x, squre_y+squre_l/2, l2A+squre_z-i); +// RoArmM2_goalPosMove(); +// delay(2); +// } +// delay(1250); + +// for(double i=0;i<=squre_l;i+=0.1){ +// RoArmM2_baseCoordinateCtrl(l3+l2B+squre_x, squre_y+squre_l/2-i, l2A+squre_z-squre_l); +// RoArmM2_goalPosMove(); +// delay(2); +// } +// delay(1250); + +// for(double i=0;i<=squre_l;i+=0.1){ +// RoArmM2_baseCoordinateCtrl(l3+l2B+squre_x, squre_y-squre_l/2, l2A+squre_z-squre_l+i); +// RoArmM2_goalPosMove(); +// delay(2); +// } +// delay(1250); +// } + + +// void RoArmM2_Test_drawCircleYZ(){ +// for(float i=0; i<=1; i+=0.001){ +// getCirclePointYZ(initY, initZ-100, 100, i); +// RoArmM2_baseCoordinateCtrl(initX-100, goalY, goalZ); +// RoArmM2_goalPosMove(); +// delay(3); +// } +// } \ No newline at end of file diff --git a/WAVE_ROVER_V1.0.ino b/WAVE_ROVER_V1.0.ino new file mode 100644 index 0000000..0c194fb --- /dev/null +++ b/WAVE_ROVER_V1.0.ino @@ -0,0 +1,267 @@ +#include +StaticJsonDocument<256> jsonCmdReceive; +StaticJsonDocument<256> jsonInfoSend; +StaticJsonDocument<512> jsonInfoHttp; + +// TaskHandle_t Pid_ctrl; + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +// 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(); +} diff --git a/battery_ctrl.h b/battery_ctrl.h new file mode 100644 index 0000000..32d0bab --- /dev/null +++ b/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(); +} \ No newline at end of file diff --git a/data/devConfig.json b/data/devConfig.json new file mode 100644 index 0000000..352146b --- /dev/null +++ b/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"} \ No newline at end of file diff --git a/data/wifiConfig.json b/data/wifiConfig.json new file mode 100644 index 0000000..352146b --- /dev/null +++ b/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"} \ No newline at end of file diff --git a/esp_now_ctrl.h b/esp_now_ctrl.h new file mode 100644 index 0000000..607e600 --- /dev/null +++ b/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; + } + } +} \ No newline at end of file diff --git a/files_ctrl.h b/files_ctrl.h new file mode 100644 index 0000000..14a9a24 --- /dev/null +++ b/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 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); + } + } + +} \ No newline at end of file diff --git a/http_server.h b/http_server.h new file mode 100644 index 0000000..a30e323 --- /dev/null +++ b/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(); +} \ No newline at end of file diff --git a/json_cmd.h b/json_cmd.h new file mode 100644 index 0000000..48c01bc --- /dev/null +++ b/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 \ No newline at end of file diff --git a/movtion_module.h b/movtion_module.h new file mode 100644 index 0000000..ea59dce --- /dev/null +++ b/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) 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"; + } +} \ No newline at end of file diff --git a/oled_ctrl.h b/oled_ctrl.h new file mode 100644 index 0000000..c8080c3 --- /dev/null +++ b/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 +#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; +} \ No newline at end of file diff --git a/uart_ctrl.h b/uart_ctrl.h new file mode 100644 index 0000000..388ec03 --- /dev/null +++ b/uart_ctrl.h @@ -0,0 +1,516 @@ +void jsonCmdReceiveHandler(){ + int cmdType = jsonCmdReceive["T"].as(); + switch(cmdType){ + case CMD_SPEED_CTRL: if (jsonCmdReceive.containsKey("T") && + jsonCmdReceive.containsKey("L") && + jsonCmdReceive.containsKey("R")){ + if (jsonCmdReceive["L"].is() && + jsonCmdReceive["R"].is()){ + 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 = ""; + } + } +} \ No newline at end of file diff --git a/ugv_advance.h b/ugv_advance.h new file mode 100644 index 0000000..c7c3d87 --- /dev/null +++ b/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); +} diff --git a/ugv_config.h b/ugv_config.h new file mode 100644 index 0000000..76a0d29 --- /dev/null +++ b/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; \ No newline at end of file diff --git a/ugv_led_ctrl.h b/ugv_led_ctrl.h new file mode 100644 index 0000000..c6ac53e --- /dev/null +++ b/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)); +} \ No newline at end of file diff --git a/web_page.h b/web_page.h new file mode 100644 index 0000000..ebb2f4a --- /dev/null +++ b/web_page.h @@ -0,0 +1,1034 @@ +const char index_html[] PROGMEM = R"rawliteral( + + + + UGV01_BASE_WEB + + + + + +
+
+
+

Control Panel

+
+
+
+
+
+
+ -1.01 + VOLTAGE +
+
+ -1.01 + RSSI +
+
+
+
+
+
+ -1.01 + ROLL +
+
+ -1.01 + PITCH +
+
+ -1.01 + YAW +
+
+ -1.01 + PAN +
+
+ -1.01 + TILT +
+
+ -1.01 + TEMP +
+
+
+
+
+
+ 192.168.10.67 + IP +
+
+ 44:17:93:EE:F8:F8 + MAC +
+
+
+
+
+
+
+
+ + + +
+
+ + + +
+
+ + + +
+
+
+
+
+
+ +
+
+
+
+
+ + + +
+
+
+
+
+ +
+
+
+
+
+ + +
+
+
+
+
+
+ + + +
+
+
+
+
+ + + +
+
+
+
+
+
+
+
+

Feedback infomation

+ Json feedback infomation shows here. +
+
+
+ + +
+
+
+

SPEED_CTRL: {"T":1,"L":0.5,"R":0.5}

+ +
+
+

PWM_INPUT: {"T":11,"L":164,"R":164}

+ +
+
+

ROS_CTRL: {"T":13,"X":0.1,"Z":0.3}

+ +
+
+

PID_SET: {"T":2,"P":200,"I":2500,"D":0,"L":255}

+ +
+
+
+
+

OLED_SET: {"T":3,"lineNum":0,"Text":"putYourTextHere"}

+ +
+
+

OLED_DEFAULT: {"T":-3}

+ +
+
+
+
+

CMD_MODULE_TYPE: {"T":4,"cmd":0}

+ +
+
+

CMD_EOAT_TYPE: {"T":124,"mode":0}

+ +
+
+

CMD_CONFIG_EOAT: {"T":125,"pos":3,"ea":0,"eb":20}

+ +
+
+
+
+

CMD_GET_IMU_DATA: {"T":126}

+ +
+
+
+
+

CMD_BASE_FEEDBACK: {"T":130}

+ +
+
+

CMD_BASE_FEEDBACK_FLOW: {"T":131,"cmd":0}

+ +
+
+

FEEDBACK_FLOW_INTERVAL: {"T":142,"cmd":0}

+ +
+
+

CMD_UART_ECHO_MODE: {"T":143,"cmd":0}

+ +
+
+

CMD_HEART_BEAT_SET: {"T":136,"cmd":0}

+ +
+
+
+
+

CMD_LED_CTRL: {"T":132,"IO4":255,"IO5":255}

+ +
+
+

CMD_GIMBAL_CTRL_SIMPLE: {"T":133,"X":45,"Y":45,"SPD":0,"ACC":0}

+ +
+
+

CMD_GIMBAL_CTRL_MOVE: {"T":134,"X":45,"Y":45,"SX":300,"SY":300}

+ +
+
+

CMD_GIMBAL_CTRL_STOP: {"T":135}

+ +
+
+

CMD_GIMBAL_STEADY: {"T":137,"s":1,"y":0}

+ +
+
+

CMD_GIMBAL_USER_CTRL: {"T":141,"s":1,"y":0}

+ +
+
+
+
+

CMD_SET_SPD_RATE: {"T":138,"L":1,"R":1}

+ +
+
+

CMD_GET_SPD_RATE: {"T":139}

+ +
+
+

CMD_SAVE_SPD_RATE: {"T":140}

+ +
+
+
+
+

CMD_MISSION_CONTENT: {"T":221,"name":"mission_a"}

+ +
+
+

CMD_APPEND_STEP_JSON: {"T":222,"name":"mission_a","step":"{\"T\":137,\"s\":1,\"y\":0}"}

+ +
+
+
+
+

CMD_BROADCAST_FOLLOWER: {"T":300,"mode":1}

+ +
+
+

CMD_ESP_NOW_CONFIG: {"T":301,"mode":3}

+ +
+
+

CMD_GET_MAC_ADDRESS: {"T":302}

+ +
+
+

CMD_ESP_NOW_ADD_FOLLOWER: {"T":303,"mac":"FF:FF:FF:FF:FF:FF"}

+ +
+
+

CMD_ESP_NOW_REMOVE_FOLLOWER: {"T":304,"mac":"FF:FF:FF:FF:FF:FF"}

+ +
+
+

CMD_ESP_NOW_GROUP_CTRL: {"T":305,"dev":0,"b":0,"s":0,"e":1.57,"h":1.57,"cmd":0,"megs":"hello!"}

+ +
+
+

CMD_ESP_NOW_SINGLE: {"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!"}

+ +
+
+
+
+

CMD_WIFI_ON_BOOT: {"T":401,"cmd":3}

+ +
+
+

CMD_SET_AP: {"T":402,"ssid":"UGV","password":"12345678"}

+ +
+
+

CMD_SET_STA: {"T":403,"ssid":"na","password":"ps"}

+ +
+
+

CMD_WIFI_APSTA: {"T":404,"ap_ssid":"UGV","ap_password":"12345678","sta_ssid":"na","sta_password":"ps"}

+ +
+
+

CMD_WIFI_INFO: {"T":405}

+ +
+
+

CMD_WIFI_CONFIG_CREATE_BY_STATUS: {"T":406}

+ +
+
+

CMD_WIFI_CONFIG_CREATE_BY_INPUT: {"T":407,"mode":3,"ap_ssid":"UGV","ap_password":"12345678","sta_ssid":"na","sta_password":"ps"}

+ +
+
+

CMD_WIFI_STOP: {"T":408}

+ +
+
+
+
+

CMD_SET_SERVO_ID: {"T":501,"raw":1,"new":11}

+ +
+
+

CMD_SET_MIDDLE: {"T":502,"id":11}

+ +
+
+

CMD_SET_SERVO_PID: {"T":503,"id":14,"p":16}

+ +
+
+
+
+

CMD_REBOOT: {"T":600}

+ +
+
+

CMD_FREE_FLASH_SPACE: {"T":601}

+ +
+
+

CMD_BOOT_MISSION_INFO: {"T":602}

+ +
+
+

CMD_RESET_BOOT_MISSION: {"T":603}

+ +
+
+

CMD_NVS_CLEAR: {"T":604}

+ +
+
+

CMD_INFO_PRINT: {"T":605,"cmd":1}

+ +
+
+
+
+

CMD_MM_TYPE_SET: {"T":900,"main":1,"module":0}

+ +
+
+
+
+
+ + + +)rawliteral"; diff --git a/wifi_ctrl.h b/wifi_ctrl.h new file mode 100644 index 0000000..96d1d4c --- /dev/null +++ b/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 +// #include +// #include + +// 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(); +} \ No newline at end of file