MrYoshii/WAVE_ROVER
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Initial import of WAVE_ROVER_V1.0

Browse Source
This commit is contained in:
Joshua Sacherer
2026-04-23 17:52:46 +02:00
commit bdd2b4315b
28 changed files with 8449 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitignore
+7
View File
@@ -0,0 +1,7 @@
.DS_Store
build/
*.elf
*.map
*.bin
*.hex
.arduino/
AK09918.cpp
+285
View File
@@ -0,0 +1,285 @@
/*
AK09918.cpp
A library for Grove - IMU 9DOF(ICM20600 + AK09918)
Copyright (c) 2018 seeed technology inc.
Website : www.seeed.cc
Author : Jerry Yip
Create Time: 2018-06
Version : 0.1
Change Log :
The MIT License (MIT)
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/
#include "AK09918.h"
AK09918::AK09918() {
_addr = AK09918_I2C_ADDR;
}
AK09918_err_type_t AK09918::initialize(AK09918_mode_type_t mode) {
if (mode == AK09918_SELF_TEST) {
mode = AK09918_POWER_DOWN;
}
_mode = mode;
if (mode == AK09918_NORMAL) {
return AK09918_ERR_OK;
} else {
return AK09918::switchMode(_mode);
}
}
AK09918_err_type_t AK09918::isDataReady() {
if (!AK09918::readByte(_addr, AK09918_ST1, _buffer)) {
return AK09918_ERR_READ_FAILED;
} else {
if (_buffer[0] & AK09918_DRDY_BIT) {
return AK09918_ERR_OK;
} else {
return AK09918_ERR_NOT_RDY;
}
}
}
AK09918_err_type_t AK09918::isDataSkip() {
if (!AK09918::readByte(_addr, AK09918_ST1, _buffer)) {
return AK09918_ERR_READ_FAILED;
} else {
if (_buffer[0] & AK09918_DOR_BIT) {
return AK09918_ERR_DOR;
} else {
return AK09918_ERR_OK;
}
}
}
AK09918_err_type_t AK09918::getData(int16_t* axis_x, int16_t* axis_y, int16_t* axis_z) {
AK09918_err_type_t err = AK09918::getRawData(axis_x, axis_y, axis_z);
(*axis_x) = (*axis_x) * 15 / 100;
(*axis_y) = (*axis_y) * 15 / 100;
(*axis_z) = (*axis_z) * 15 / 100;
return err;
}
AK09918_err_type_t AK09918::getRawData(int16_t* axis_x, int16_t* axis_y, int16_t* axis_z) {
if (_mode == AK09918_NORMAL) {
AK09918::switchMode(AK09918_NORMAL);
bool is_end = false;
int count = 0;
while (!is_end) {
if (AK09918::_getRawMode() == 0x00) {
is_end = true;
}
if (count >= 15) {
return AK09918_ERR_TIMEOUT;
}
count ++;
delay(1);
}
}
if (!AK09918::readBytes(_addr, AK09918_HXL, 8, _buffer)) {
return AK09918_ERR_READ_FAILED;
} else {
*axis_x = (_buffer[1] << 8 | _buffer[0]);
*axis_y = (_buffer[3] << 8 | _buffer[2]);
*axis_z = (_buffer[5] << 8 | _buffer[4]);
if (_buffer[7] & AK09918_HOFL_BIT) {
return AK09918_ERR_OVERFLOW;
}
return AK09918_ERR_OK;
}
}
AK09918_mode_type_t AK09918::getMode() {
return _mode;
}
AK09918_err_type_t AK09918::switchMode(AK09918_mode_type_t mode) {
if (mode == AK09918_SELF_TEST) {
return AK09918_ERR_WRITE_FAILED;
}
_mode = mode;
if (!AK09918::writeByte(_addr, AK09918_CNTL2, mode)) {
return AK09918_ERR_WRITE_FAILED;
}
return AK09918_ERR_OK;
}
// 1.Set Power-down mode. (MODE[4:0] bits = “00000”)
// 2.Set Self-test mode. (MODE[4:0] bits = “10000”)
// 3.Check Data Ready or not by polling DRDY bit of ST1 register.
// 4.When Data Ready, proceed to the next step. Read measurement data. (HXL to HZH)
AK09918_err_type_t AK09918::selfTest() {
int32_t axis_x, axis_y, axis_z;
bool is_end = false;
AK09918_err_type_t err;
if (!AK09918::writeByte(_addr, AK09918_CNTL2, AK09918_POWER_DOWN)) {
return AK09918_ERR_WRITE_FAILED;
}
delay(1);
if (!AK09918::writeByte(_addr, AK09918_CNTL2, AK09918_SELF_TEST)) {
return AK09918_ERR_WRITE_FAILED;
}
while (!is_end) {
err = AK09918::isDataReady();
if (err == AK09918_ERR_OK) {
is_end = true;
} else if (err == AK09918_ERR_READ_FAILED) {
return AK09918_ERR_READ_FAILED;
}
delay(1);
}
// read data and check
if (!AK09918::readBytes(_addr, AK09918_HXL, 8, _buffer)) {
return AK09918_ERR_READ_FAILED;
} else {
axis_x = (int32_t)((((int16_t)_buffer[1]) << 8) | _buffer[0]);
axis_y = (int32_t)((((int16_t)_buffer[3]) << 8) | _buffer[2]);
axis_z = (int32_t)((((int16_t)_buffer[5]) << 8) | _buffer[4]);
if ((axis_x >= -200) && (axis_x <= 200) && (axis_y >= -200) && (axis_y <= 200) && \
(axis_z >= -1000) && (axis_z <= -150)) {
return AK09918_ERR_OK;
} else {
return AK09918_ERR_SELFTEST_FAILED;
}
}
}
AK09918_err_type_t AK09918::reset() {
if (!AK09918::writeByte(_addr, AK09918_CNTL3, AK09918_SRST_BIT)) {
return AK09918_ERR_WRITE_FAILED;
}
return AK09918_ERR_OK;
}
String AK09918::strError(AK09918_err_type_t err) {
String result;
switch (err) {
case AK09918_ERR_OK:
result = "AK09918_ERR_OK: OK";
break;
case AK09918_ERR_DOR:
result = "AK09918_ERR_DOR: Data skipped";
break;
case AK09918_ERR_NOT_RDY:
result = "AK09918_ERR_NOT_RDY: Not ready";
break;
case AK09918_ERR_TIMEOUT:
result = "AK09918_ERR_TIMEOUT: Timeout";
break;
case AK09918_ERR_SELFTEST_FAILED:
result = "AK09918_ERR_SELFTEST_FAILED: Self test failed";
break;
case AK09918_ERR_OVERFLOW:
result = "AK09918_ERR_OVERFLOW: Sensor overflow";
break;
case AK09918_ERR_WRITE_FAILED:
result = "AK09918_ERR_WRITE_FAILED: Fail to write";
break;
case AK09918_ERR_READ_FAILED:
result = "AK09918_ERR_READ_FAILED: Fail to read";
break;
default:
result = "Unknown Error";
break;
}
return result;
}
uint16_t AK09918::getDeviceID() {
AK09918::readBytes(_addr, AK09918_WIA1, 2, _buffer);
return (((uint16_t)_buffer[0]) << 8) | _buffer[1];
}
uint8_t AK09918::_getRawMode() {
if (!AK09918::readByte(0x0c, AK09918_CNTL2, _buffer)) {
return 0xFF;
} else {
return _buffer[0];
}
}
bool AK09918::readBytes(uint8_t addr,uint8_t reg,uint8_t num,uint8_t *buf)
{
Wire.beginTransmission(addr);
Wire.write(reg);
if (Wire.endTransmission(false) != 0) {
return false;
}
if (Wire.requestFrom((int)addr, (int)num) != num) {
return false;
}
for (int i = 0; i < num; i++)
{
if (!Wire.available()) {
return false;
}
buf[i] = (uint8_t)Wire.read();
}
return true;
}
bool AK09918::readByte(uint8_t addr,uint8_t reg ,uint8_t *buf)
{
Wire.beginTransmission(addr);
Wire.write(reg);
if (Wire.endTransmission(false) != 0) {
return false;
}
if (Wire.requestFrom((int)addr, 1) != 1) {
return false;
}
if (!Wire.available()) {
return false;
}
buf[0] = (uint8_t)Wire.read();
return true;
}
bool AK09918::writeByte(uint8_t addr,uint8_t reg ,uint8_t Value)
{
Wire.beginTransmission(addr);
Wire.write(reg);
Wire.write(Value);
return Wire.endTransmission() == 0;
}
AK09918.h
+149
View File
@@ -0,0 +1,149 @@
/*
AK09918.h
A library for Grove - IMU 9DOF(ICM20600 + AK09918)
Copyright (c) 2018 seeed technology inc.
Website : www.seeed.cc
Author : Jerry Yip
Create Time: 2018-06
Version : 0.1
Change Log :
The MIT License (MIT)
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/
#ifndef __IMU_9DOF_AK09918_H__
#define __IMU_9DOF_AK09918_H__
#include <Arduino.h>
#include <Wire.h>
/***************************************************************
AK09918 I2C Register
***************************************************************/
#define AK09918_I2C_ADDR 0x0c // I2C address (Can't be changed)
#define AK09918_WIA1 0x00 // Company ID
#define AK09918_WIA2 0x01 // Device ID
#define AK09918_RSV1 0x02 // Reserved 1
#define AK09918_RSV2 0x03 // Reserved 2
#define AK09918_ST1 0x10 // DataStatus 1
#define AK09918_HXL 0x11 // X-axis data
#define AK09918_HXH 0x12
#define AK09918_HYL 0x13 // Y-axis data
#define AK09918_HYH 0x14
#define AK09918_HZL 0x15 // Z-axis data
#define AK09918_HZH 0x16
#define AK09918_TMPS 0x17 // Dummy
#define AK09918_ST2 0x18 // Datastatus 2
#define AK09918_CNTL1 0x30 // Dummy
#define AK09918_CNTL2 0x31 // Control settings
#define AK09918_CNTL3 0x32 // Control settings
#define AK09918_SRST_BIT 0x01 // Soft Reset
#define AK09918_HOFL_BIT 0x08 // Sensor Over Flow
#define AK09918_DOR_BIT 0x02 // Data Over Run
#define AK09918_DRDY_BIT 0x01 // Data Ready
// #define AK09918_MEASURE_PERIOD 9 // Must not be changed
// AK09918 has following seven operation modes:
// (1) Power-down mode: AK09918 doesn't measure
// (2) Single measurement mode: measure when you call any getData() function
// (3) Continuous measurement mode 1: 10Hz, measure 10 times per second,
// (4) Continuous measurement mode 2: 20Hz, measure 20 times per second,
// (5) Continuous measurement mode 3: 50Hz, measure 50 times per second,
// (6) Continuous measurement mode 4: 100Hz, measure 100 times per second,
// (7) Self-test mode
enum AK09918_mode_type_t {
AK09918_POWER_DOWN = 0x00,
AK09918_NORMAL = 0x01,
AK09918_CONTINUOUS_10HZ = 0x02,
AK09918_CONTINUOUS_20HZ = 0x04,
AK09918_CONTINUOUS_50HZ = 0x06,
AK09918_CONTINUOUS_100HZ = 0x08,
AK09918_SELF_TEST = 0x10, // ignored by switchMode() and initialize(), call selfTest() to use this mode
};
enum AK09918_err_type_t {
AK09918_ERR_OK = 0, // ok
AK09918_ERR_DOR = 1, // data skipped
AK09918_ERR_NOT_RDY = 2, // not ready
AK09918_ERR_TIMEOUT = 3, // read/write timeout
AK09918_ERR_SELFTEST_FAILED = 4, // self test failed
AK09918_ERR_OVERFLOW = 5, // sensor overflow, means |x|+|y|+|z| >= 4912uT
AK09918_ERR_WRITE_FAILED = 6, // fail to write
AK09918_ERR_READ_FAILED = 7, // fail to read
};
typedef struct imu_st_sensor_data_tag
{
short int s16X;
short int s16Y;
short int s16Z;
}IMU_ST_SENSOR_DATA;
class AK09918 {
public:
AK09918();
// default to AK09918_CONTINUOUS_10HZ mode
AK09918_err_type_t initialize(AK09918_mode_type_t mode = AK09918_NORMAL);
// At AK09918_CONTINUOUS_** mode, check if data is ready to read
AK09918_err_type_t isDataReady();
// At AK09918_CONTINUOUS_** mode, check if data is skipped
AK09918_err_type_t isDataSkip();
// Get magnet data in uT
AK09918_err_type_t getData(int16_t* axis_x, int16_t* axis_y, int16_t* axis_z);
// Get raw I2C magnet data
AK09918_err_type_t getRawData(int16_t* axis_x, int16_t* axis_y, int16_t* axis_z);
// Return the working mode of AK09918
AK09918_mode_type_t getMode();
// Switch the working mode of AK09918
AK09918_err_type_t switchMode(AK09918_mode_type_t mode);
// Start a self-test, if pass, return AK09918_ERR_OK
AK09918_err_type_t selfTest();
// Reset AK09918
AK09918_err_type_t reset();
// Get details of AK09918_err_type_t
String strError(AK09918_err_type_t err);
// Get device ID
uint16_t getDeviceID();
private:
uint8_t _getRawMode();
bool writeByte(uint8_t addr,uint8_t reg ,uint8_t Value);
bool readByte(uint8_t addr,uint8_t reg,uint8_t *buf);
bool readBytes(uint8_t addr,uint8_t reg,uint8_t num,uint8_t *buf);
uint8_t _addr;
AK09918_mode_type_t _mode;
uint8_t _buffer[16];
};
#endif // __IMU_9DOF_AK09918_H__
IMU.cpp
+359
View File
@@ -0,0 +1,359 @@
#include "IMU.h"
void calibrateMagn();
void imuAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
float invSqrt(float x);
/******************************************************************************
* IMU module *
******************************************************************************/
// #define S_SCL 33
// #define S_SDA 32
AK09918_err_type_t err;
QMI8658 qmi8658_;
AK09918 magnetometer_;
int16_t offset_x = -12, offset_y = 0, offset_z = 0;
int16_t x, y, z;
// Find the magnetic declination at your location
// http://www.magnetic-declination.com/
double declination_shenzhen = -3.22;
#define Kp 4.50f // proportional gain governs rate of convergence to accelerometer/magnetometer
#define Ki 1.0f // integral gain governs rate of convergence of gyroscope biases
float angles[3];
float q0, q1, q2, q3;
float exInt = 0.0f;
float eyInt = 0.0f;
float ezInt = 0.0f;
uint32_t lastFilterUpdateUs = 0;
namespace {
constexpr float kDegToRad = 0.01745329251994329577f;
constexpr float kDefaultSampleDt = 0.01f;
constexpr float kMaxSampleDt = 0.1f;
float clampUnit(float value)
{
if (value > 1.0f) {
return 1.0f;
}
if (value < -1.0f) {
return -1.0f;
}
return value;
}
void resetFilterState()
{
q0 = 1.0f;
q1 = 0.0f;
q2 = 0.0f;
q3 = 0.0f;
exInt = 0.0f;
eyInt = 0.0f;
ezInt = 0.0f;
lastFilterUpdateUs = micros();
}
float getSampleDeltaSeconds()
{
const uint32_t nowUs = micros();
if (lastFilterUpdateUs == 0) {
lastFilterUpdateUs = nowUs;
return kDefaultSampleDt;
}
const uint32_t elapsedUs = nowUs - lastFilterUpdateUs;
lastFilterUpdateUs = nowUs;
const float deltaSeconds = elapsedUs * 1.0e-6f;
if (deltaSeconds <= 0.0f || deltaSeconds > kMaxSampleDt) {
return kDefaultSampleDt;
}
return deltaSeconds;
}
bool normalizeVector(float *xAxis, float *yAxis, float *zAxis)
{
const float normSquared = (*xAxis * *xAxis) + (*yAxis * *yAxis) + (*zAxis * *zAxis);
if (normSquared <= 1.0e-12f) {
return false;
}
const float norm = invSqrt(normSquared);
*xAxis *= norm;
*yAxis *= norm;
*zAxis *= norm;
return true;
}
} // namespace
void imuInit()
{
// Wire.begin(S_SDA, S_SCL);
// Serial.begin(115200);
if (qmi8658_.begin() == 0)
Serial.println("qmi8658_init fail");
if (magnetometer_.initialize())
Serial.println("AK09918_init fail") ;
magnetometer_.switchMode(AK09918_CONTINUOUS_100HZ);
err = magnetometer_.isDataReady();
int retry_times = 0;
while (err != AK09918_ERR_OK) {
Serial.println(err);
Serial.println("Waiting Sensor");
delay(100);
magnetometer_.reset();
delay(100);
magnetometer_.switchMode(AK09918_CONTINUOUS_100HZ);
err = magnetometer_.isDataReady();
retry_times ++;
if (retry_times > 10) {
break;
}
}
// Serial.println("Start figure-8 calibration after 1 seconds.");
// delay(1000);
// calibrate(10000, &offset_x, &offset_y, &offset_z);
// calibrateMagn();
resetFilterState();
}
void imuDataGet(EulerAngles *pstAngles,
IMU_ST_SENSOR_DATA_FLOAT *pstGyroRawData,
IMU_ST_SENSOR_DATA_FLOAT *pstAccelRawData,
IMU_ST_SENSOR_DATA *pstMagnRawData)
{
if (pstAngles == nullptr || pstGyroRawData == nullptr ||
pstAccelRawData == nullptr || pstMagnRawData == nullptr) {
return;
}
float acc[3] = {0.0f, 0.0f, 0.0f};
float gyro[3] = {0.0f, 0.0f, 0.0f};
float MotionVal[9];
const AK09918_err_type_t magErr = magnetometer_.getData(&x, &y, &z);
if (magErr == AK09918_ERR_OVERFLOW) {
Serial.println("AK09918 overflow detected, keeping last valid magnetic sample.");
}
pstMagnRawData->s16X = x- offset_x;
pstMagnRawData->s16Y = y- offset_y;
pstMagnRawData->s16Z = z- offset_z;
// qmi8658_.GetEulerAngles(&pstAngles->pitch,&pstAngles->roll,&pstAngles->yaw,acc,gyro);
qmi8658_.read_sensor_data(acc,gyro);
// pstAngles->roll = atan2((float)acc[1], (float)acc[2]);
// pstAngles->pitch = atan2(-(float)acc[0], sqrt((float)(acc[1] * acc[1]) + (float)(acc[2] * acc[2])));
// double Xheading = pstMagnRawData->s16X * cos(pstAngles->pitch) + pstMagnRawData->s16Y * sin(pstAngles->roll) * sin(pstAngles->pitch) + pstMagnRawData->s16Z * cos(pstAngles->roll) * sin(pstAngles->pitch);
// double Yheading = pstMagnRawData->s16Y * cos(pstAngles->roll) - pstMagnRawData->s16Z * sin(pstAngles->pitch);
// pstAngles->yaw = /*180 + */57.3 * atan2(Yheading, Xheading) + declination_shenzhen;
// pstAngles->roll = atan2((float)acc[1], (float)acc[2]) * 57.3;
// pstAngles->pitch = atan2(-(float)acc[0], sqrt((float)(acc[1] * acc[1]) + (float)(acc[2] * acc[2]))) * 57.3;
MotionVal[0]=gyro[0];
MotionVal[1]=gyro[1];
MotionVal[2]=gyro[2];
MotionVal[3]=acc[0];
MotionVal[4]=acc[1];
MotionVal[5]=acc[2];
MotionVal[6]=pstMagnRawData->s16X;
MotionVal[7]=pstMagnRawData->s16Y;
MotionVal[8]=pstMagnRawData->s16Z;
imuAHRSupdate((float)MotionVal[0] * kDegToRad, (float)MotionVal[1] * kDegToRad, (float)MotionVal[2] * kDegToRad,
(float)MotionVal[3], (float)MotionVal[4], (float)MotionVal[5],
(float)MotionVal[6], (float)MotionVal[7], MotionVal[8]);
pstAngles->pitch = asinf(clampUnit(-2.0f * q1 * q3 + 2.0f * q0 * q2)) * 57.2957795f;
pstAngles->roll = atan2f(2.0f * q2 * q3 + 2.0f * q0 * q1,
-2.0f * q1 * q1 - 2.0f * q2 * q2 + 1.0f) * 57.2957795f;
pstAngles->yaw = atan2f(-2.0f * q1 * q2 - 2.0f * q0 * q3,
2.0f * q2 * q2 + 2.0f * q3 * q3 - 1.0f) * 57.2957795f;
pstGyroRawData->X = gyro[0];
pstGyroRawData->Y = gyro[1];
pstGyroRawData->Z = gyro[2];
pstAccelRawData->X = acc[0];
pstAccelRawData->Y = acc[1];
pstAccelRawData->Z = acc[2];
return;
}
bool imuRecalibrate()
{
qmi8658_.autoOffsets();
resetFilterState();
return true;
}
void imuGetMagnOffsets(IMU_ST_SENSOR_DATA *pstMagnOffset)
{
if (pstMagnOffset == nullptr) {
return;
}
pstMagnOffset->s16X = offset_x;
pstMagnOffset->s16Y = offset_y;
pstMagnOffset->s16Z = offset_z;
}
void imuSetMagnOffsets(int16_t inputX, int16_t inputY, int16_t inputZ)
{
offset_x = inputX;
offset_y = inputY;
offset_z = inputZ;
resetFilterState();
}
float imuGetTemperature()
{
return qmi8658_.read_temperature();
}
void imuAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
{
const float sampleDt = getSampleDeltaSeconds();
const float halfT = 0.5f * sampleDt;
float hx = 0.0f, hy = 0.0f, hz = 0.0f, bx = 0.0f, bz = 0.0f;
float vx = 0.0f, vy = 0.0f, vz = 0.0f, wx = 0.0f, wy = 0.0f, wz = 0.0f;
float ex = 0.0f, ey = 0.0f, ez = 0.0f;
float q0q0 = q0 * q0;
float q0q1 = q0 * q1;
float q0q2 = q0 * q2;
float q0q3 = q0 * q3;
float q1q1 = q1 * q1;
float q1q2 = q1 * q2;
float q1q3 = q1 * q3;
float q2q2 = q2 * q2;
float q2q3 = q2 * q3;
float q3q3 = q3 * q3;
const bool accelValid = normalizeVector(&ax, &ay, &az);
const bool magnValid = normalizeVector(&mx, &my, &mz);
if (accelValid) {
vx = 2.0f * (q1q3 - q0q2);
vy = 2.0f * (q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3;
ex += (ay * vz - az * vy);
ey += (az * vx - ax * vz);
ez += (ax * vy - ay * vx);
}
if (magnValid) {
hx = 2.0f * mx * (0.5f - q2q2 - q3q3) + 2.0f * my * (q1q2 - q0q3) + 2.0f * mz * (q1q3 + q0q2);
hy = 2.0f * mx * (q1q2 + q0q3) + 2.0f * my * (0.5f - q1q1 - q3q3) + 2.0f * mz * (q2q3 - q0q1);
hz = 2.0f * mx * (q1q3 - q0q2) + 2.0f * my * (q2q3 + q0q1) + 2.0f * mz * (0.5f - q1q1 - q2q2);
bx = sqrtf((hx * hx) + (hy * hy));
bz = hz;
wx = 2.0f * bx * (0.5f - q2q2 - q3q3) + 2.0f * bz * (q1q3 - q0q2);
wy = 2.0f * bx * (q1q2 - q0q3) + 2.0f * bz * (q0q1 + q2q3);
wz = 2.0f * bx * (q0q2 + q1q3) + 2.0f * bz * (0.5f - q1q1 - q2q2);
ex += (my * wz - mz * wy);
ey += (mz * wx - mx * wz);
ez += (mx * wy - my * wx);
}
if (accelValid || magnValid) {
exInt += ex * Ki * sampleDt;
eyInt += ey * Ki * sampleDt;
ezInt += ez * Ki * sampleDt;
gx = gx + Kp * ex + exInt;
gy = gy + Kp * ey + eyInt;
gz = gz + Kp * ez + ezInt;
}
const float q0Prev = q0;
const float q1Prev = q1;
const float q2Prev = q2;
const float q3Prev = q3;
q0 = q0Prev + (-q1Prev * gx - q2Prev * gy - q3Prev * gz) * halfT;
q1 = q1Prev + (q0Prev * gx + q2Prev * gz - q3Prev * gy) * halfT;
q2 = q2Prev + (q0Prev * gy - q1Prev * gz + q3Prev * gx) * halfT;
q3 = q3Prev + (q0Prev * gz + q1Prev * gy - q2Prev * gx) * halfT;
const float normSquared = q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3;
if (normSquared <= 1.0e-12f) {
resetFilterState();
return;
}
const float norm = invSqrt(normSquared);
q0 = q0 * norm;
q1 = q1 * norm;
q2 = q2 * norm;
q3 = q3 * norm;
}
float invSqrt(float x)
{
if (x <= 0.0f) {
return 0.0f;
}
float halfx = 0.5f * x;
union {
float f;
uint32_t i;
} conv = { x };
conv.i = 0x5f3759df - (conv.i >> 1);
float y = conv.f;
y = y * (1.5f - (halfx * y * y));
return y;
}
void calibrateMagn(void)
{
int16_t temp[9];
Serial.printf("keep 10dof-imu device horizontal and it will read x y z axis offset value after 4 seconds\n");
delay(4000);
Serial.printf("start read all axises offset value\n");
magnetometer_.getData(&x, &y, &z);
temp[0] = x;
temp[1] = y;
temp[2] = z;
Serial.printf("rotate z axis 180 degrees and it will read all axises offset value after 4 seconds\n");
delay(4000);
Serial.printf("start read all axises offset value\n");
magnetometer_.getData(&x, &y, &z);
temp[3] = x;
temp[4] = y;
temp[5] = z;
Serial.printf("flip 10dof-imu device and keep it horizontal and it will read all axises offset value after 4 seconds\n");
delay(4000);
Serial.printf("start read all axises offset value\n");
magnetometer_.getData(&x, &y, &z);
temp[6] = x;
temp[7] = y;
temp[8] = z;
offset_x = (temp[0]+temp[3])/2;
offset_y = (temp[1]+temp[4])/2;
offset_z = (temp[5]+temp[8])/2;
}
IMU.h
+35
View File
@@ -0,0 +1,35 @@
#ifndef _IMU_H_
#define _IMU_H_
#include "AK09918.h"
#include "QMI8658.h"
#include <stdio.h>
#include <math.h>
typedef struct imu_st_angles_data_tag
{
float fYaw;
float fPitch;
float fRoll;
}IMU_ST_ANGLES_DATA;
typedef struct imu_st_sensor_data_float
{
float X;
float Y;
float Z;
}IMU_ST_SENSOR_DATA_FLOAT;
void imuInit();
void imuDataGet(EulerAngles *pstAngles,
IMU_ST_SENSOR_DATA_FLOAT *pstGyroRawData,
IMU_ST_SENSOR_DATA_FLOAT *pstAccelRawData,
IMU_ST_SENSOR_DATA *pstMagnRawData);
bool imuRecalibrate();
void imuGetMagnOffsets(IMU_ST_SENSOR_DATA *pstMagnOffset);
void imuSetMagnOffsets(int16_t offsetX, int16_t offsetY, int16_t offsetZ);
float imuGetTemperature();
void imuAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
float invSqrt(float x);
#endif
IMU_ctrl.h
+103
View File
@@ -0,0 +1,103 @@
#include"IMU.h"
// define GPIOs for IIC.
EulerAngles stAngles;
IMU_ST_SENSOR_DATA_FLOAT stGyroRawData;
IMU_ST_SENSOR_DATA_FLOAT stAccelRawData;
IMU_ST_SENSOR_DATA stMagnRawData;
float temp;
void imu_init() {
imuInit();
}
void updateIMUData() {
imuDataGet( &stAngles, &stGyroRawData, &stAccelRawData, &stMagnRawData);
temp = imuGetTemperature();
ax = stAccelRawData.X;
ay = stAccelRawData.Y;
az = stAccelRawData.Z;
mx = stMagnRawData.s16X;
my = stMagnRawData.s16Y;
mz = stMagnRawData.s16Z;
gx = stGyroRawData.X;
gy = stGyroRawData.Y;
gz = stGyroRawData.Z;
icm_roll = stAngles.roll;
icm_pitch = stAngles.pitch;
icm_yaw = stAngles.yaw;
icm_temp = temp;
last_imu_update = millis();
}
void imuCalibration() {
const bool calibrationOk = imuRecalibrate();
updateIMUData();
jsonInfoHttp.clear();
jsonInfoHttp["T"] = FEEDBACK_IMU_DATA;
jsonInfoHttp["status"] = calibrationOk ? 1 : 0;
jsonInfoHttp["info"] = calibrationOk ? "IMU calibration finished." : "IMU calibration failed.";
jsonInfoHttp["r"] = icm_roll;
jsonInfoHttp["p"] = icm_pitch;
jsonInfoHttp["y"] = icm_yaw;
String getInfoJsonString;
serializeJson(jsonInfoHttp, getInfoJsonString);
Serial.println(getInfoJsonString);
}
void getIMUData() {
jsonInfoHttp.clear();
jsonInfoHttp["T"] = FEEDBACK_IMU_DATA;
jsonInfoHttp["r"] = icm_roll;
jsonInfoHttp["p"] = icm_pitch;
jsonInfoHttp["y"] = icm_yaw;
jsonInfoHttp["ax"] = ax;
jsonInfoHttp["ay"] = ay;
jsonInfoHttp["az"] = az;
jsonInfoHttp["gx"] = gx;
jsonInfoHttp["gy"] = gy;
jsonInfoHttp["gz"] = gz;
jsonInfoHttp["mx"] = mx;
jsonInfoHttp["my"] = my;
jsonInfoHttp["mz"] = mz;
jsonInfoHttp["temp"] = temp;
String getInfoJsonString;
serializeJson(jsonInfoHttp, getInfoJsonString);
Serial.println(getInfoJsonString);
}
void getIMUOffset() {
IMU_ST_SENSOR_DATA offsetData;
imuGetMagnOffsets(&offsetData);
jsonInfoHttp.clear();
jsonInfoHttp["T"] = CMD_GET_IMU_OFFSET;
jsonInfoHttp["x"] = offsetData.s16X;
jsonInfoHttp["y"] = offsetData.s16Y;
jsonInfoHttp["z"] = offsetData.s16Z;
String getInfoJsonString;
serializeJson(jsonInfoHttp, getInfoJsonString);
Serial.println(getInfoJsonString);
}
void setIMUOffset(int16_t inputX, int16_t inputY, int16_t inputZ) {
imuSetMagnOffsets(inputX, inputY, inputZ);
getIMUOffset();
}
QMI8658.cpp
+559
View File
@@ -0,0 +1,559 @@
#include "QMI8658.h"
#include <Wire.h>
#include <math.h>
#define QMI8658_UINT_MG_DPS
//#define M_PI (3.14159265358979323846f)
#define ONE_G (9.807f)
static qmi8658_state g_imu;
namespace {
uint8_t qmi8658_address()
{
return g_imu.slave ? g_imu.slave : QMI8658_ADDR;
}
}
void QMI8658::write_reg(uint8_t reg,uint8_t value)
{
Wire.beginTransmission(qmi8658_address());
Wire.write(reg);
Wire.write(value);
last_status = Wire.endTransmission();
}
uint8_t QMI8658::read_reg(uint8_t reg)
{
uint8_t ret = 0;
unsigned int retry = 0;
while(retry++ < 5)
{
Wire.beginTransmission(qmi8658_address());
Wire.write(reg);
last_status = Wire.endTransmission(false);
if (last_status != 0) {
continue;
}
if (Wire.requestFrom((int)qmi8658_address(), 1) != 1) {
continue;
}
if (Wire.available()) {
ret = (uint8_t)Wire.read();
break;
}
}
return ret;
}
uint16_t QMI8658::readWord_reg(uint8_t reg)
{
uint8_t retH=0;
uint8_t retL=0;
Wire.beginTransmission(qmi8658_address());
Wire.write(reg);
last_status = Wire.endTransmission(false);
if (last_status != 0) {
return 0;
}
if (Wire.requestFrom((int)qmi8658_address(), 2) != 2) {
return 0;
}
if (Wire.available() < 2) {
return 0;
}
retL = (uint8_t)Wire.read();
retH = (uint8_t)Wire.read();
return ((retH << 8) | retL);
}
void QMI8658::read_sensor_data(float acc[3], float gyro[3])
{
unsigned char buf_reg[12];
short raw_acc_xyz[3];
short raw_gyro_xyz[3];
raw_acc_xyz[0] = (short)((unsigned short)( readWord_reg(Qmi8658Register_Ax_L) ));
raw_acc_xyz[1] = (short)((unsigned short)( readWord_reg(Qmi8658Register_Ay_L) ));
raw_acc_xyz[2] = (short)((unsigned short)( readWord_reg(Qmi8658Register_Az_L) ));
raw_gyro_xyz[0] = (short)((unsigned short)( readWord_reg(Qmi8658Register_Gx_L) ));
raw_gyro_xyz[1] = (short)((unsigned short)( readWord_reg(Qmi8658Register_Gy_L) ));
raw_gyro_xyz[2] = (short)((unsigned short)( readWord_reg(Qmi8658Register_Gz_L) ));
#if defined(QMI8658_UINT_MG_DPS)
// mg
// Serial.println("mg");
acc[0] = (float)(raw_acc_xyz[0]*1000.0f)/g_imu.ssvt_a - TempAcc.X_Off_Err;
acc[1] = (float)(raw_acc_xyz[1]*1000.0f)/g_imu.ssvt_a - TempAcc.Y_Off_Err;
acc[2] = (float)(raw_acc_xyz[2]*1000.0f)/g_imu.ssvt_a - TempAcc.Z_Off_Err;
#else
// m/s2
// Serial.println("m/s2");
acc[0] = (float)(raw_acc_xyz[0]*ONE_G)/g_imu.ssvt_a;
acc[1] = (float)(raw_acc_xyz[1]*ONE_G)/g_imu.ssvt_a;
acc[2] = (float)(raw_acc_xyz[2]*ONE_G)/g_imu.ssvt_a;
#endif
#if defined(QMI8658_UINT_MG_DPS)
// dps
// Serial.println("dps");
gyro[0] = (float)(raw_gyro_xyz[0]*1.0f)/g_imu.ssvt_g - TempGyr.X_Off_Err;
gyro[1] = (float)(raw_gyro_xyz[1]*1.0f)/g_imu.ssvt_g - TempGyr.Y_Off_Err;
gyro[2] = (float)(raw_gyro_xyz[2]*1.0f)/g_imu.ssvt_g - TempGyr.Z_Off_Err;
#else
// rad/s
// Serial.println("rad/s");
gyro[0] = (float)(raw_gyro_xyz[0]*M_PI)/(g_imu.ssvt_g*180); // *pi/180
gyro[1] = (float)(raw_gyro_xyz[1]*M_PI)/(g_imu.ssvt_g*180);
gyro[2] = (float)(raw_gyro_xyz[2]*M_PI)/(g_imu.ssvt_g*180);
#endif
}
void QMI8658::read_acc(float acc[3])
{
unsigned char buf_reg[12];
short raw_acc_xyz[3];
raw_acc_xyz[0] = (short)((unsigned short)( readWord_reg(Qmi8658Register_Ax_L) ));
raw_acc_xyz[1] = (short)((unsigned short)( readWord_reg(Qmi8658Register_Ay_L) ));
raw_acc_xyz[2] = (short)((unsigned short)( readWord_reg(Qmi8658Register_Az_L) ));
#if defined(QMI8658_UINT_MG_DPS)
// mg
acc[0] = (float)(raw_acc_xyz[0]*1000.0f)/g_imu.ssvt_a;
acc[1] = (float)(raw_acc_xyz[1]*1000.0f)/g_imu.ssvt_a;
acc[2] = (float)(raw_acc_xyz[2]*1000.0f)/g_imu.ssvt_a;
#else
// m/s2
// Serial.println("m/s2");
acc[0] = (float)(raw_acc_xyz[0]*ONE_G)/g_imu.ssvt_a;
acc[1] = (float)(raw_acc_xyz[1]*ONE_G)/g_imu.ssvt_a;
acc[2] = (float)(raw_acc_xyz[2]*ONE_G)/g_imu.ssvt_a;
#endif
}
void QMI8658::read_gyro(float gyro[3])
{
unsigned char buf_reg[12];
short raw_gyro_xyz[3];
raw_gyro_xyz[0] = (short)((unsigned short)( readWord_reg(Qmi8658Register_Gx_L) ));
raw_gyro_xyz[1] = (short)((unsigned short)( readWord_reg(Qmi8658Register_Gy_L) ));
raw_gyro_xyz[2] = (short)((unsigned short)( readWord_reg(Qmi8658Register_Gz_L) ));
#if defined(QMI8658_UINT_MG_DPS)
// dps
gyro[0] = (float)(raw_gyro_xyz[0]*1.0f)/g_imu.ssvt_g;
gyro[1] = (float)(raw_gyro_xyz[1]*1.0f)/g_imu.ssvt_g;
gyro[2] = (float)(raw_gyro_xyz[2]*1.0f)/g_imu.ssvt_g;
#else
// rad/s
// Serial.println("rad/s");
gyro[0] = (float)(raw_gyro_xyz[0]*M_PI)/(g_imu.ssvt_g*180); // *pi/180
gyro[1] = (float)(raw_gyro_xyz[1]*M_PI)/(g_imu.ssvt_g*180);
gyro[2] = (float)(raw_gyro_xyz[2]*M_PI)/(g_imu.ssvt_g*180);
#endif
}
float QMI8658::read_temperature()
{
const int16_t raw_temp = (int16_t)((unsigned short)(readWord_reg(Qmi8658Register_Tempearture_L)));
return raw_temp / 256.0f;
}
void QMI8658::axis_convert(float data_a[3], float data_g[3], int layout)
{
float raw[3],raw_g[3];
raw[0] = data_a[0];
raw[1] = data_a[1];
//raw[2] = data[2];
raw_g[0] = data_g[0];
raw_g[1] = data_g[1];
//raw_g[2] = data_g[2];
if(layout >=4 && layout <= 7)
{
data_a[2] = -data_a[2];
data_g[2] = -data_g[2];
}
if(layout%2)
{
data_a[0] = raw[1];
data_a[1] = raw[0];
data_g[0] = raw_g[1];
data_g[1] = raw_g[0];
}
else
{
data_a[0] = raw[0];
data_a[1] = raw[1];
data_g[0] = raw_g[0];
data_g[1] = raw_g[1];
}
if((layout==1)||(layout==2)||(layout==4)||(layout==7))
{
data_a[0] = -data_a[0];
data_g[0] = -data_g[0];
}
if((layout==2)||(layout==3)||(layout==6)||(layout==7))
{
data_a[1] = -data_a[1];
data_g[1] = -data_g[1];
}
}
void QMI8658::read_xyz(float acc[3], float gyro[3])
{
unsigned char status;
unsigned char data_ready = 0;
#if defined(QMI8658_SYNC_SAMPLE_MODE)
status = read_reg(Qmi8658Register_StatusInt);
if(status&0x01)
{
data_ready = 1;
delayMicroseconds(6);
}
#else
status = read_reg(Qmi8658Register_Status0);
if(status&0x03)
{
data_ready = 1;
}
#endif
if(data_ready)
{
read_sensor_data(acc, gyro);
axis_convert(acc, gyro, 0);
#if defined(QMI8658_USE_CALI)
qmi8658_data_cali(1, acc);
qmi8658_data_cali(2, gyro);
#endif
g_imu.imu[0] = acc[0];
g_imu.imu[1] = acc[1];
g_imu.imu[2] = acc[2];
g_imu.imu[3] = gyro[0];
g_imu.imu[4] = gyro[1];
g_imu.imu[5] = gyro[2];
}
else
{
acc[0] = g_imu.imu[0];
acc[1] = g_imu.imu[1];
acc[2] = g_imu.imu[2];
gyro[0] = g_imu.imu[3];
gyro[1] = g_imu.imu[4];
gyro[2] = g_imu.imu[5];
Serial.print("data ready fail!\n");
}
}
void QMI8658::config_acc(enum qmi8658_AccRange range, enum qmi8658_AccOdr odr, enum qmi8658_LpfConfig lpfEnable, enum qmi8658_StConfig stEnable)
{
unsigned char ctl_dada;
switch(range)
{
case Qmi8658AccRange_2g:
g_imu.ssvt_a = (1<<14);
break;
case Qmi8658AccRange_4g:
g_imu.ssvt_a = (1<<13);
break;
case Qmi8658AccRange_8g:
g_imu.ssvt_a = (1<<12);
break;
case Qmi8658AccRange_16g:
g_imu.ssvt_a = (1<<11);
break;
default:
range = Qmi8658AccRange_8g;
g_imu.ssvt_a = (1<<12);
}
if(stEnable == Qmi8658St_Enable)
ctl_dada = (unsigned char)range|(unsigned char)odr|0x80;
else
ctl_dada = (unsigned char)range|(unsigned char)odr;
write_reg(Qmi8658Register_Ctrl2, ctl_dada);
// set LPF & HPF
ctl_dada = read_reg(Qmi8658Register_Ctrl5);
ctl_dada &= 0xf0;
if(lpfEnable == Qmi8658Lpf_Enable)
{
ctl_dada |= A_LSP_MODE_3;
ctl_dada |= 0x01;
}
else
{
ctl_dada &= ~0x01;
}
//ctl_dada = 0x00;
write_reg(Qmi8658Register_Ctrl5,ctl_dada);
// set LPF & HPF
}
void QMI8658::config_gyro(enum qmi8658_GyrRange range, enum qmi8658_GyrOdr odr, enum qmi8658_LpfConfig lpfEnable, enum qmi8658_StConfig stEnable)
{
// Set the CTRL3 register to configure dynamic range and ODR
unsigned char ctl_dada;
// Store the scale factor for use when processing raw data
switch (range)
{
case Qmi8658GyrRange_16dps:
g_imu.ssvt_g = 2048;
break;
case Qmi8658GyrRange_32dps:
g_imu.ssvt_g = 1024;
break;
case Qmi8658GyrRange_64dps:
g_imu.ssvt_g = 512;
break;
case Qmi8658GyrRange_128dps:
g_imu.ssvt_g = 256;
break;
case Qmi8658GyrRange_256dps:
g_imu.ssvt_g = 128;
break;
case Qmi8658GyrRange_512dps:
g_imu.ssvt_g = 64;
break;
case Qmi8658GyrRange_1024dps:
g_imu.ssvt_g = 32;
break;
case Qmi8658GyrRange_2048dps:
g_imu.ssvt_g = 16;
break;
// case Qmi8658GyrRange_4096dps:
// g_imu.ssvt_g = 8;
// break;
default:
range = Qmi8658GyrRange_512dps;
g_imu.ssvt_g = 64;
break;
}
if(stEnable == Qmi8658St_Enable)
ctl_dada = (unsigned char)range|(unsigned char)odr|0x80;
else
ctl_dada = (unsigned char)range | (unsigned char)odr;
write_reg(Qmi8658Register_Ctrl3, ctl_dada);
// Conversion from degrees/s to rad/s if necessary
// set LPF & HPF
ctl_dada = read_reg(Qmi8658Register_Ctrl5);
ctl_dada &= 0x0f;
if(lpfEnable == Qmi8658Lpf_Enable)
{
ctl_dada |= G_LSP_MODE_3;
ctl_dada |= 0x10;
}
else
{
ctl_dada &= ~0x10;
}
//ctl_dada = 0x00;
write_reg(Qmi8658Register_Ctrl5,ctl_dada);
// set LPF & HPF
}
void QMI8658::enableSensors(unsigned char enableFlags)
{
#if defined(QMI8658_SYNC_SAMPLE_MODE)
write_reg(Qmi8658Register_Ctrl7, enableFlags | 0x80);
#elif defined(QMI8658_USE_FIFO)
//qmi8658_write_reg(Qmi8658Register_Ctrl7, enableFlags|QMI8658_DRDY_DISABLE);
write_reg(Qmi8658Register_Ctrl7, enableFlags);
#else
write_reg(Qmi8658Register_Ctrl7, enableFlags);
#endif
g_imu.cfg.enSensors = enableFlags&0x03;
delay(1);
}
void QMI8658::config_reg(unsigned char low_power)
{
enableSensors(QMI8658_DISABLE_ALL);
if(low_power)
{
g_imu.cfg.enSensors = QMI8658_ACC_ENABLE;
g_imu.cfg.accRange = Qmi8658AccRange_8g;
g_imu.cfg.accOdr = Qmi8658AccOdr_LowPower_21Hz;
g_imu.cfg.gyrRange = Qmi8658GyrRange_1024dps;
g_imu.cfg.gyrOdr = Qmi8658GyrOdr_250Hz;
}
else
{
g_imu.cfg.enSensors = QMI8658_ACCGYR_ENABLE;
g_imu.cfg.accRange = Qmi8658AccRange_16g;
g_imu.cfg.accOdr = Qmi8658AccOdr_1000Hz;
g_imu.cfg.gyrRange = Qmi8658GyrRange_2048dps;
g_imu.cfg.gyrOdr = Qmi8658GyrOdr_1000Hz;
}
if(g_imu.cfg.enSensors & QMI8658_ACC_ENABLE)
{
config_acc(g_imu.cfg.accRange, g_imu.cfg.accOdr, Qmi8658Lpf_Disable, Qmi8658St_Disable);
}
if(g_imu.cfg.enSensors & QMI8658_GYR_ENABLE)
{
config_gyro(g_imu.cfg.gyrRange, g_imu.cfg.gyrOdr, Qmi8658Lpf_Disable, Qmi8658St_Disable);
}
}
unsigned char QMI8658::get_id(void)
{
unsigned char qmi8658_chip_id = 0x00;
unsigned char qmi8658_revision_id = 0x00;
unsigned char qmi8658_slave[2] = {QMI8658_SLAVE_ADDR_L, QMI8658_SLAVE_ADDR_H};
int retry = 0;
unsigned char iCount = 0;
unsigned char firmware_id[3];
unsigned char uuid[6];
unsigned int uuid_low, uuid_high;
while(iCount<2)
{
g_imu.slave = qmi8658_slave[iCount];
retry = 0;
while((qmi8658_chip_id != 0x05)&&(retry++ < 5))
{
qmi8658_chip_id = read_reg(Qmi8658Register_WhoAmI);
Serial.printf("Qmi8658Register_WhoAmI = 0x%x\n", qmi8658_chip_id);
}
if(qmi8658_chip_id == 0x05)
{
qmi8658_on_demand_cali();
g_imu.cfg.ctrl8_value = 0xc0;
//QMI8658_INT1_ENABLE, QMI8658_INT2_ENABLE
write_reg(Qmi8658Register_Ctrl1, 0x60|QMI8658_INT2_ENABLE|QMI8658_INT1_ENABLE);
qmi8658_revision_id = read_reg(Qmi8658Register_Revision);
// qmi8658_read_reg(Qmi8658Register_firmware_id, firmware_id, 3);
// qmi8658_read_reg(Qmi8658Register_uuid, uuid, 6);
write_reg(Qmi8658Register_Ctrl7, 0x00);
write_reg(Qmi8658Register_Ctrl8, g_imu.cfg.ctrl8_value);
// uuid_low = (unsigned int)((unsigned int)(uuid[2]<<16)|(unsigned int)(uuid[1]<<8)|(uuid[0]));
// uuid_high = (unsigned int)((unsigned int)(uuid[5]<<16)|(unsigned int)(uuid[4]<<8)|(uuid[3]));
// qmi8658_log("qmi8658_init slave=0x%x Revision=0x%x\n", g_imu.slave, qmi8658_revision_id);
// qmi8658_log("Firmware ID[0x%x 0x%x 0x%x]\n", firmware_id[2], firmware_id[1],firmware_id[0]);
// qmi8658_log("UUID[0x%x %x]\n", uuid_high ,uuid_low);
break;
}
iCount++;
}
return qmi8658_chip_id;
}
void QMI8658::qmi8658_on_demand_cali(void)
{
Serial.print("qmi8658_on_demand_cali start\n");
write_reg(Qmi8658Register_Reset, 0xb0);
delay(10); // delay
write_reg(Qmi8658Register_Ctrl9, (unsigned char)qmi8658_Ctrl9_Cmd_On_Demand_Cali);
delay(2200); // delay 2000ms above
write_reg(Qmi8658Register_Ctrl9, (unsigned char)qmi8658_Ctrl9_Cmd_NOP);
delay(100); // delay
Serial.print("qmi8658_on_demand_cali done\n");
}
unsigned char QMI8658::begin(void)
{
if(get_id() == 0x05)
{
#if defined(QMI8658_USE_AMD)
qmi8658_config_amd();
#endif
#if defined(QMI8658_USE_PEDOMETER)
qmi8658_config_pedometer(125);
qmi8658_enable_pedometer(1);
#endif
config_reg(0);
enableSensors(g_imu.cfg.enSensors);
Serial.println("Position your ICM20948 flat and don't move it - calibrating...");
delay(1000);
autoOffsets();
// Serial.print();
dump_reg();
#if defined(QMI8658_USE_CALI)
memset(&g_cali, 0, sizeof(g_cali));
#endif
return 1;
}
else
{
// Serial.print("qmi8658_init fail\n");
return 0;
}
}
void QMI8658::dump_reg(void)
{
// unsigned char read_data[8];
// qmi8658_read_reg(Qmi8658Register_Ctrl1, read_data, 8);
// qmi8658_log("Ctrl1[0x%x]\nCtrl2[0x%x]\nCtrl3[0x%x]\nCtrl4[0x%x]\nCtrl5[0x%x]\nCtrl6[0x%x]\nCtrl7[0x%x]\nCtrl8[0x%x]\n",
// read_data[0],read_data[1],read_data[2],read_data[3],read_data[4],read_data[5],read_data[6],read_data[7]);
}
void QMI8658::autoOffsets(void){
float acc[3],gyro[3];
TempAcc.X_Off_Err = 0.0f;
TempAcc.Y_Off_Err = 0.0f;
TempAcc.Z_Off_Err = 0.0f;
TempGyr.X_Off_Err = 0.0f;
TempGyr.Y_Off_Err = 0.0f;
TempGyr.Z_Off_Err = 0.0f;
for(int i=0; i<50; i++){
QMI8658::read_acc(acc);
TempAcc.X_Off_Err += acc[0];
TempAcc.Y_Off_Err += acc[1];
TempAcc.Z_Off_Err += acc[2];
delay(10);
}
TempAcc.X_Off_Err /= 50;
TempAcc.Y_Off_Err /= 50;
TempAcc.Z_Off_Err /= 50;
TempAcc.Z_Off_Err -= 1000.0f;
for(int i=0; i<50; i++){
QMI8658::read_gyro(gyro);
TempGyr.X_Off_Err += gyro[0];
TempGyr.Y_Off_Err += gyro[1];
TempGyr.Z_Off_Err += gyro[2];
delay(1);
}
TempGyr.X_Off_Err /= 50;
TempGyr.Y_Off_Err /= 50;
TempGyr.Z_Off_Err /= 50;
}
QMI8658.h
+102
View File
@@ -0,0 +1,102 @@
/*
* @Description: QMI8658
* @Author: zjw
* @Date: 2022-10-24
* @LastEditTime: 2022-10-24
* @LastEditors: zjw
*/
#ifndef _QMI8658_H_
#define _QMI8658_H_
#include <Arduino.h>
#include "QMI8658reg.h"
typedef struct
{
float roll;
float pitch;
float yaw ;
} EulerAngles;
typedef struct
{
float X_Off_Err;
float Y_Off_Err;
float Z_Off_Err;
}QMI8658_TypeDef_Off;
class QMI8658
{
uint8_t last_status; // status of last I2C transmission
uint8_t read_reg(uint8_t reg);
void write_reg(uint8_t reg,uint8_t value);
public:
uint16_t readWord_reg(uint8_t reg);
// bool init(void);
// bool GetEulerAngles(float *pitch,float *roll, float *yaw,float acc[3],float gyro[3]);
void config_acc(enum qmi8658_AccRange range, enum qmi8658_AccOdr odr,
enum qmi8658_LpfConfig lpfEnable, enum qmi8658_StConfig stEnable);
void config_gyro(enum qmi8658_GyrRange range, enum qmi8658_GyrOdr odr,
enum qmi8658_LpfConfig lpfEnable, enum qmi8658_StConfig stEnable);
void read_sensor_data(float acc[3], float gyro[3]);
void read_acc(float acc[3]);
void read_gyro(float gyro[3]);
float read_temperature();
void read_xyz(float acc[3], float gyro[3]);
void axis_convert(float data_a[3], float data_g[3], int layout);
void config_reg(unsigned char low_power);
void enableSensors(unsigned char enableFlags);
unsigned char get_id(void);
unsigned char begin(void);
void dump_reg(void);
void qmi8658_on_demand_cali(void);
void autoOffsets(void);
public:
int16_t ax, ay, az, gx, gy, gz;
float pith, roll, yaw;
unsigned long now, lastTime = 0;
float dt; //微分时间
float agz = 0; //角度变量
long gzo = 0; //陀螺仪偏移量
QMI8658_TypeDef_Off TempAcc = {0};//校准值
QMI8658_TypeDef_Off TempGyr = {0};
};
/*----------------------------------------------------------------------------------------------
QMI8658C UI Sensor Configuration Settings and Output Data
*/
///<Configuration Registers>
#define QMI8658_ADDR 0X6B //device address
#define WHO_AM_I 0X00 //Device identifier
#define CTRL1 0x02 //Serial Interface and Sensor Enable
#define CTRL2 0x03 //Accelerometer Settings
#define CTRL3 0x04 //Gyroscope Settings
#define CTRL4 0X05 //Magnetometer Settings
#define CTRL5 0X06 //Sensor Data Processing Settings
#define CTRL7 0x08 //Enable Sensors and Configure Data Reads
#define CTRL8 0X09 //Reserved – Special Settings
///<Sensor Data Output Registers>
#define AccX_L 0x35
#define AccX_H 0x36
#define AccY_L 0x37
#define AccY_H 0x38
#define AccZ_L 0x39
#define AccZ_H 0x3A
#define TEMP_L 0x33
#define GyrX_L 0x3B
#define GyrX_H 0x3C
#define GyrY_L 0x3D
#define GyrY_H 0x3E
#define GyrZ_L 0x3F
#define GyrZ_H 0x40
// int16_t QMI8658C_readBytes(unsigned char tmp);
//extern QMI8658C _QMI8658C;
#endif
QMI8658reg.h
+344
View File
@@ -0,0 +1,344 @@
#ifndef _QMI8658REG_H_
#define _QMI8658REG_H_
// #define QMI8658_USE_SPI
//#define QMI8658_SYNC_SAMPLE_MODE
//#define QMI8658_SOFT_SELFTEST
//#define QMI8658_USE_CALI
#define QMI8658_USE_FIFO
//#define QMI8658_USE_AMD
//#define QMI8658_USE_PEDOMETER
#define QMI8658_SLAVE_ADDR_L 0x6a
#define QMI8658_SLAVE_ADDR_H 0x6b
#define QMI8658_DISABLE_ALL (0x0)
#define QMI8658_ACC_ENABLE (0x1)
#define QMI8658_GYR_ENABLE (0x2)
#define QMI8658_ACCGYR_ENABLE (QMI8658_ACC_ENABLE | QMI8658_GYR_ENABLE)
#define QMI8658_STATUS1_CMD_DONE (0x01)
#define QMI8658_STATUS1_WAKEUP_EVENT (0x04)
#define QMI8658_CTRL8_DATAVALID_EN 0x40 // bit6:1 int1, 0 int2
#define QMI8658_CTRL8_PEDOMETER_EN 0x10
#define QMI8658_CTRL8_SIGMOTION_EN 0x08
#define QMI8658_CTRL8_NOMOTION_EN 0x04
#define QMI8658_CTRL8_ANYMOTION_EN 0x02
#define QMI8658_CTRL8_TAP_EN 0x01
#define QMI8658_INT1_ENABLE 0x08
#define QMI8658_INT2_ENABLE 0x10
#define QMI8658_DRDY_DISABLE 0x20 // ctrl7
#define QMI8658_FIFO_MAP_INT1 0x04 // ctrl1
#define QMI8658_FIFO_MAP_INT2 ~0x04 // ctrl1
#define qmi8658_log printf
enum Qmi8658Register
{
Qmi8658Register_WhoAmI = 0,
Qmi8658Register_Revision,
Qmi8658Register_Ctrl1,
Qmi8658Register_Ctrl2,
Qmi8658Register_Ctrl3,
Qmi8658Register_Ctrl4,
Qmi8658Register_Ctrl5,
Qmi8658Register_Ctrl6,
Qmi8658Register_Ctrl7,
Qmi8658Register_Ctrl8,
Qmi8658Register_Ctrl9,
Qmi8658Register_Cal1_L = 11,
Qmi8658Register_Cal1_H,
Qmi8658Register_Cal2_L,
Qmi8658Register_Cal2_H,
Qmi8658Register_Cal3_L,
Qmi8658Register_Cal3_H,
Qmi8658Register_Cal4_L,
Qmi8658Register_Cal4_H,
Qmi8658Register_FifoWmkTh = 19,
Qmi8658Register_FifoCtrl = 20,
Qmi8658Register_FifoCount = 21,
Qmi8658Register_FifoStatus = 22,
Qmi8658Register_FifoData = 23,
Qmi8658Register_StatusI2CM = 44,
Qmi8658Register_StatusInt = 45,
Qmi8658Register_Status0,
Qmi8658Register_Status1,
Qmi8658Register_Timestamp_L = 48,
Qmi8658Register_Timestamp_M,
Qmi8658Register_Timestamp_H,
Qmi8658Register_Tempearture_L = 51,
Qmi8658Register_Tempearture_H,
Qmi8658Register_Ax_L = 53,
Qmi8658Register_Ax_H,
Qmi8658Register_Ay_L,
Qmi8658Register_Ay_H,
Qmi8658Register_Az_L,
Qmi8658Register_Az_H,
Qmi8658Register_Gx_L = 59,
Qmi8658Register_Gx_H,
Qmi8658Register_Gy_L,
Qmi8658Register_Gy_H,
Qmi8658Register_Gz_L,
Qmi8658Register_Gz_H,
Qmi8658Register_Mx_L = 65,
Qmi8658Register_Mx_H,
Qmi8658Register_My_L,
Qmi8658Register_My_H,
Qmi8658Register_Mz_L,
Qmi8658Register_Mz_H,
Qmi8658Register_firmware_id = 73,
Qmi8658Register_uuid = 81,
Qmi8658Register_Pedo_L = 90,
Qmi8658Register_Pedo_M = 91,
Qmi8658Register_Pedo_H = 92,
Qmi8658Register_Reset = 96
};
enum qmi8658_Ois_Register
{
qmi8658_OIS_Reg_Ctrl1 = 0x02,
qmi8658_OIS_Reg_Ctrl2,
qmi8658_OIS_Reg_Ctrl3,
qmi8658_OIS_Reg_Ctrl5 = 0x06,
qmi8658_OIS_Reg_Ctrl7 = 0x08,
qmi8658_OIS_Reg_StatusInt = 0x2D,
qmi8658_OIS_Reg_Status0 = 0x2E,
qmi8658_OIS_Reg_Ax_L = 0x33,
qmi8658_OIS_Reg_Ax_H,
qmi8658_OIS_Reg_Ay_L,
qmi8658_OIS_Reg_Ay_H,
qmi8658_OIS_Reg_Az_L,
qmi8658_OIS_Reg_Az_H,
qmi8658_OIS_Reg_Gx_L = 0x3B,
qmi8658_OIS_Reg_Gx_H,
qmi8658_OIS_Reg_Gy_L,
qmi8658_OIS_Reg_Gy_H,
qmi8658_OIS_Reg_Gz_L,
qmi8658_OIS_Reg_Gz_H,
};
enum qmi8658_Ctrl9Command
{
qmi8658_Ctrl9_Cmd_NOP = 0X00,
qmi8658_Ctrl9_Cmd_GyroBias = 0X01,
qmi8658_Ctrl9_Cmd_Rqst_Sdi_Mod = 0X03,
qmi8658_Ctrl9_Cmd_Rst_Fifo = 0X04,
qmi8658_Ctrl9_Cmd_Req_Fifo = 0X05,
qmi8658_Ctrl9_Cmd_I2CM_Write = 0X06,
qmi8658_Ctrl9_Cmd_WoM_Setting = 0x08,
qmi8658_Ctrl9_Cmd_AccelHostDeltaOffset = 0x09,
qmi8658_Ctrl9_Cmd_GyroHostDeltaOffset = 0x0A,
qmi8658_Ctrl9_Cmd_EnableExtReset = 0x0B,
qmi8658_Ctrl9_Cmd_EnableTap = 0x0C,
qmi8658_Ctrl9_Cmd_EnablePedometer = 0x0D,
qmi8658_Ctrl9_Cmd_Motion = 0x0E,
qmi8658_Ctrl9_Cmd_CopyUsid = 0x10,
qmi8658_Ctrl9_Cmd_SetRpu = 0x11,
qmi8658_Ctrl9_Cmd_On_Demand_Cali = 0xA2,
qmi8658_Ctrl9_Cmd_Dbg_WoM_Data_Enable = 0xF8
};
enum qmi8658_LpfConfig
{
Qmi8658Lpf_Disable,
Qmi8658Lpf_Enable
};
enum qmi8658_HpfConfig
{
Qmi8658Hpf_Disable,
Qmi8658Hpf_Enable
};
enum qmi8658_StConfig
{
Qmi8658St_Disable,
Qmi8658St_Enable
};
enum qmi8658_LpfMode
{
A_LSP_MODE_0 = 0x00<<1,
A_LSP_MODE_1 = 0x01<<1,
A_LSP_MODE_2 = 0x02<<1,
A_LSP_MODE_3 = 0x03<<1,
G_LSP_MODE_0 = 0x00<<5,
G_LSP_MODE_1 = 0x01<<5,
G_LSP_MODE_2 = 0x02<<5,
G_LSP_MODE_3 = 0x03<<5
};
enum qmi8658_AccRange
{
Qmi8658AccRange_2g = 0x00 << 4,
Qmi8658AccRange_4g = 0x01 << 4,
Qmi8658AccRange_8g = 0x02 << 4,
Qmi8658AccRange_16g = 0x03 << 4
};
enum qmi8658_AccOdr
{
Qmi8658AccOdr_8000Hz = 0x00,
Qmi8658AccOdr_4000Hz = 0x01,
Qmi8658AccOdr_2000Hz = 0x02,
Qmi8658AccOdr_1000Hz = 0x03,
Qmi8658AccOdr_500Hz = 0x04,
Qmi8658AccOdr_250Hz = 0x05,
Qmi8658AccOdr_125Hz = 0x06,
Qmi8658AccOdr_62_5Hz = 0x07,
Qmi8658AccOdr_31_25Hz = 0x08,
Qmi8658AccOdr_LowPower_128Hz = 0x0c,
Qmi8658AccOdr_LowPower_21Hz = 0x0d,
Qmi8658AccOdr_LowPower_11Hz = 0x0e,
Qmi8658AccOdr_LowPower_3Hz = 0x0f
};
enum qmi8658_GyrRange
{
Qmi8658GyrRange_16dps = 0 << 4,
Qmi8658GyrRange_32dps = 1 << 4,
Qmi8658GyrRange_64dps = 2 << 4,
Qmi8658GyrRange_128dps = 3 << 4,
Qmi8658GyrRange_256dps = 4 << 4,
Qmi8658GyrRange_512dps = 5 << 4,
Qmi8658GyrRange_1024dps = 6 << 4,
Qmi8658GyrRange_2048dps = 7 << 4
};
/*!
* \brief Gyroscope output rate configuration.
*/
enum qmi8658_GyrOdr
{
Qmi8658GyrOdr_8000Hz = 0x00,
Qmi8658GyrOdr_4000Hz = 0x01,
Qmi8658GyrOdr_2000Hz = 0x02,
Qmi8658GyrOdr_1000Hz = 0x03,
Qmi8658GyrOdr_500Hz = 0x04,
Qmi8658GyrOdr_250Hz = 0x05,
Qmi8658GyrOdr_125Hz = 0x06,
Qmi8658GyrOdr_62_5Hz = 0x07,
Qmi8658GyrOdr_31_25Hz = 0x08
};
enum qmi8658_AccUnit
{
Qmi8658AccUnit_g,
Qmi8658AccUnit_ms2
};
enum qmi8658_GyrUnit
{
Qmi8658GyrUnit_dps,
Qmi8658GyrUnit_rads
};
enum qmi8658_FifoMode
{
qmi8658_Fifo_Bypass = 0,
qmi8658_Fifo_Fifo = 1,
qmi8658_Fifo_Stream = 2,
qmi8658_Fifo_StreamToFifo = 3
};
enum qmi8658_FifoWmkLevel
{
qmi8658_Fifo_WmkEmpty = (0 << 4),
qmi8658_Fifo_WmkOneQuarter = (1 << 4),
qmi8658_Fifo_WmkHalf = (2 << 4),
qmi8658_Fifo_WmkThreeQuarters = (3 << 4)
};
enum qmi8658_FifoSize
{
qmi8658_Fifo_16 = (0 << 2),
qmi8658_Fifo_32 = (1 << 2),
qmi8658_Fifo_64 = (2 << 2),
qmi8658_Fifo_128 = (3 << 2)
};
enum qmi8658_Interrupt
{
qmi8658_Int_none,
qmi8658_Int1,
qmi8658_Int2,
qmi8658_Int_total
};
enum qmi8658_InterruptState
{
Qmi8658State_high = (1 << 7),
Qmi8658State_low = (0 << 7)
};
#define QMI8658_CALI_DATA_NUM 200
typedef struct qmi8658_cali
{
float acc_last[3];
float acc[3];
float acc_fix[3];
float acc_bias[3];
float acc_sum[3];
float gyr_last[3];
float gyr[3];
float gyr_fix[3];
float gyr_bias[3];
float gyr_sum[3];
unsigned char imu_static_flag;
unsigned char acc_fix_flag;
unsigned char gyr_fix_flag;
char acc_fix_index;
unsigned char gyr_fix_index;
unsigned char acc_cali_flag;
unsigned char gyr_cali_flag;
unsigned short acc_cali_num;
unsigned short gyr_cali_num;
// unsigned char acc_avg_num;
// unsigned char gyr_avg_num;
} qmi8658_cali;
typedef struct
{
unsigned char enSensors;
enum qmi8658_AccRange accRange;
enum qmi8658_AccOdr accOdr;
enum qmi8658_GyrRange gyrRange;
enum qmi8658_GyrOdr gyrOdr;
unsigned char ctrl8_value;
#if defined(QMI8658_USE_FIFO)
unsigned char fifo_ctrl;
#endif
} qmi8658_config;
typedef struct
{
unsigned char slave;
qmi8658_config cfg;
unsigned short ssvt_a;
unsigned short ssvt_g;
unsigned int timestamp;
unsigned int step;
float imu[6];
} qmi8658_state;
#endif
README.md
+13
View File
@@ -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
RoArm-M2_module.h
+1303
View File
File diff suppressed because it is too large Load Diff
WAVE_ROVER_V1.0.ino
+267
View File
@@ -0,0 +1,267 @@
#include <ArduinoJson.h>
StaticJsonDocument<256> jsonCmdReceive;
StaticJsonDocument<256> jsonInfoSend;
StaticJsonDocument<512> jsonInfoHttp;
// TaskHandle_t Pid_ctrl;
#include <SCServo.h>
#include <nvs_flash.h>
#include <esp_system.h>
#include <LittleFS.h>
#include <WiFi.h>
#include <WebServer.h>
#include <esp_now.h>
#include <nvs_flash.h>
#include <Adafruit_SSD1306.h>
#include <INA219_WE.h>
#include <ESP32Encoder.h>
#include <PID_v2.h>
#include <SimpleKalmanFilter.h>
#include <math.h>
#include <Adafruit_ICM20X.h>
#include <Adafruit_ICM20948.h>
#include <Adafruit_Sensor.h>
// functions for barrery info.
#include "battery_ctrl.h"
// functions for oled.
#include "oled_ctrl.h"
// config for ugv.
#include "ugv_config.h"
// functions for the leds of UGV.
#include "ugv_led_ctrl.h"
// functions for RoArm-M2 ctrl.
#include "RoArm-M2_module.h"
// functions for gimbal ctrl.
#include "gimbal_module.h"
// define json cmd.
#include "json_cmd.h"
// functions for IMU ctrl.
#include "IMU_ctrl.h"
// functions for movtion ctrl.
#include "movtion_module.h"
// functions for editing the files in flash.
#include "files_ctrl.h"
// advance functions for ugv ctrl.
#include "ugv_advance.h"
// functions for wifi ctrl.
#include "wifi_ctrl.h"
// functions for esp-now.
#include "esp_now_ctrl.h"
// functions for uart json ctrl.
#include "uart_ctrl.h"
// functions for http & web server.
#include "http_server.h"
void moduleType_RoArmM2() {
unsigned long curr_time = millis();
if (curr_time - prev_time >= 10){
constantHandle();
prev_time = curr_time;
}
RoArmM2_getPosByServoFeedback();
// esp-now flow ctrl as a flow-leader.
switch(espNowMode) {
case 1: espNowGroupDevsFlowCtrl();break;
case 2: espNowSingleDevFlowCtrl();break;
}
if (InfoPrint == 2) {
RoArmM2_infoFeedback();
}
}
void moduleType_Gimbal() {
getGimbalFeedback();
gimbalSteady(steadyGoalY);
}
void setup() {
Serial.begin(115200);
Wire.begin(S_SDA, S_SCL);
while(!Serial) {}
ina219_init();
inaDataUpdate();
// set mainType & moduleType.
// mainType: 1.WAVE ROVER, 2.UGV02, 3.UGV01
// moduleType: 0.Null, 1.RoArm, 2.PT
mm_settings(mainType, moduleType);
init_oled();
if (mainType == 1) {
screenLine_0 = "WAVE ROVER";
} else if (mainType == 2) {
screenLine_0 = "UGV";
} else if (mainType == 3) {
screenLine_0 = "UGV";
}
screenLine_1 = "version: 1.00";
screenLine_2 = "starting...";
screenLine_3 = "";
oled_update();
delay(1200);
// functions for IMU.
imu_init();
// functions for the leds on ugv.
led_pin_init();
// init the littleFS funcs in files_ctrl.h
screenLine_2 = screenLine_3;
screenLine_3 = "Initialize LittleFS";
oled_update();
if(InfoPrint == 1){Serial.println("Initialize LittleFS for Flash files ctrl.");}
initFS();
// init the funcs in switch_module.h
screenLine_2 = screenLine_3;
screenLine_3 = "Initialize 12V-switch ctrl";
oled_update();
if(InfoPrint == 1){Serial.println("Initialize the pins used for 12V-switch ctrl.");}
movtionPinInit();
// servos power up
screenLine_2 = screenLine_3;
screenLine_3 = "Power up the servos";
oled_update();
if(InfoPrint == 1){Serial.println("Power up the servos.");}
delay(500);
// init servo ctrl functions.
screenLine_2 = screenLine_3;
screenLine_3 = "ServoCtrl init UART2TTL...";
oled_update();
if(InfoPrint == 1){Serial.println("ServoCtrl init UART2TTL...");}
RoArmM2_servoInit();
// check the status of the servos.
screenLine_2 = screenLine_3;
screenLine_3 = "Bus servos status check...";
oled_update();
if(InfoPrint == 1){Serial.println("Bus servos status check...");}
RoArmM2_initCheck(false);
if(InfoPrint == 1 && RoArmM2_initCheckSucceed){
Serial.println("All bus servos status checked.");
}
if(RoArmM2_initCheckSucceed) {
screenLine_2 = "Bus servos: succeed";
} else {
screenLine_2 = "Bus servos: " +
servoFeedback[BASE_SERVO_ID - 11].status +
servoFeedback[SHOULDER_DRIVING_SERVO_ID - 11].status +
servoFeedback[SHOULDER_DRIVEN_SERVO_ID - 11].status +
servoFeedback[ELBOW_SERVO_ID - 11].status +
servoFeedback[GRIPPER_SERVO_ID - 11].status;
}
screenLine_3 = ">>> Moving to init pos...";
oled_update();
RoArmM2_resetPID();
RoArmM2_moveInit();
screenLine_3 = "Reset joint torque to ST_TORQUE_MAX";
oled_update();
if(InfoPrint == 1){Serial.println("Reset joint torque to ST_TORQUE_MAX.");}
RoArmM2_dynamicAdaptation(0, ST_TORQUE_MAX, ST_TORQUE_MAX, ST_TORQUE_MAX, ST_TORQUE_MAX);
screenLine_3 = "WiFi init";
oled_update();
if(InfoPrint == 1){Serial.println("WiFi init.");}
initWifi();
screenLine_3 = "http & web init";
oled_update();
if(InfoPrint == 1){Serial.println("http & web init.");}
initHttpWebServer();
screenLine_3 = "ESP-NOW init";
oled_update();
if(InfoPrint == 1){Serial.println("ESP-NOW init.");}
initEspNow();
screenLine_3 = "UGV started";
oled_update();
if(InfoPrint == 1){Serial.println("UGV started.");}
getThisDevMacAddress();
updateOledWifiInfo();
initEncoders();
pidControllerInit();
screenLine_2 = String("MAC:") + macToString(thisDevMac);
oled_update();
led_pwm_ctrl(0, 0);
if(InfoPrint == 1){Serial.println("Application initialization settings.");}
createMission("boot", "these cmds run automatically at boot.");
missionPlay("boot", 1);
}
void loop() {
serialCtrl();
server.handleClient();
// read and compute the info of joints.
switch (moduleType) {
case 1: moduleType_RoArmM2();break;
case 2: moduleType_Gimbal();break;
}
// recv esp-now json cmd.
if(runNewJsonCmd) {
jsonCmdReceiveHandler();
jsonCmdReceive.clear();
runNewJsonCmd = false;
}
getLeftSpeed();
LeftPidControllerCompute();
getRightSpeed();
RightPidControllerCompute();
oledInfoUpdate();
updateIMUData();
if (baseFeedbackFlow) {
baseInfoFeedback();
}
heartBeatCtrl();
size_t freeHeap = esp_get_free_heap_size();
}
battery_ctrl.h
+28
View File
@@ -0,0 +1,28 @@
#define INA219_ADDRESS 0x42
INA219_WE ina219 = INA219_WE(INA219_ADDRESS);
float shuntVoltage_mV = 0.0;
float loadVoltage_V = 0.0;
float busVoltage_V = 0.0;
float current_mA = 0.0;
float power_mW = 0.0;
bool ina219_overflow = false;
void ina219_init(){
if(!ina219.init()){
Serial.println("INA219 not connected!");
}
ina219.setADCMode(BIT_MODE_9);
ina219.setPGain(PG_320);
ina219.setBusRange(BRNG_16);
ina219.setShuntSizeInOhms(0.01); // used in INA219.
}
void inaDataUpdate(){
shuntVoltage_mV = ina219.getShuntVoltage_mV();
busVoltage_V = ina219.getBusVoltage_V();
current_mA = ina219.getCurrent_mA();
power_mW = ina219.getBusPower();
loadVoltage_V = busVoltage_V + (shuntVoltage_mV/1000);
ina219_overflow = ina219.getOverflow();
}
data/devConfig.json
+1
View File
@@ -0,0 +1 @@
{"wifi_mode_on_boot":3,"sta_ssid":"JSBZY-2.4G","sta_password":"waveshare0755","ap_ssid":"RoArm","ap_password":"12345678"}
data/wifiConfig.json
+1
View File
@@ -0,0 +1 @@
{"wifi_mode_on_boot":3,"sta_ssid":"JSBZY-2.4G","sta_password":"waveshare0755","ap_ssid":"RoArm","ap_password":"12345678"}
esp_now_ctrl.h
+430
View File
@@ -0,0 +1,430 @@
const int MAX_FOLLOWERS = 10; // Maximum number of follower devices
typedef struct struct_message {
byte devCode;
float base;
float shoulder;
float elbow;
float hand;
byte cmd;
char message[210];
} struct_message;
struct_message espNowMessage;
struct_message espNowMegsRecv;
esp_now_peer_info_t peerInfo;
#define ESP_NOW_CHANNEL 0
#define ESP_NOW_ENCRYPT false
uint8_t thisDevMac[6];
uint8_t singleFollowerDev[6];
// input a mac[6]:{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF}
// return String: "FF:FF:FF:FF:FF:FF"
String macToString(uint8_t mac[6]) {
char macStr[18]; // 6 pairs of 2 characters + null terminator
snprintf(macStr, sizeof(macStr), "%02X:%02X:%02X:%02X:%02X:%02X",
mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
return String(macStr);
}
void getThisDevMacAddress() {
WiFi.macAddress(thisDevMac);
thisMacStr = macToString(thisDevMac);
jsonInfoHttp.clear();
jsonInfoHttp["mac"] = thisMacStr;
Serial.println(thisMacStr);
}
void changeEspNowMode(byte inputMode) {
switch(inputMode) {
case 0: espNowMode = inputMode;
if (InfoPrint == 1) {Serial.println("esp-now mode: none");}
screenLine_3 = "ESP-NOW: NONE";
oled_update();
jsonInfoHttp.clear();
jsonInfoHttp["info"] = "esp-now mode: none";
break;
case 1: espNowMode = inputMode;
espNowMessage.cmd = 0;
if (InfoPrint == 1) {Serial.println("esp-now mode: flow-leader(group)");}
screenLine_3 = "ESP-NOW: F-LEADER-B";
oled_update();
jsonInfoHttp.clear();
jsonInfoHttp["info"] = "esp-now mode: flow-leader(group)";
break;
case 2: espNowMode = inputMode;
espNowMessage.cmd = 0;
if (InfoPrint == 1) {Serial.println("esp-now mode: flow-leader(single)");}
screenLine_3 = "ESP-NOW: F-LEADER-S";
oled_update();
jsonInfoHttp.clear();
jsonInfoHttp["info"] = "esp-now mode: flow-leader(single)";
break;
case 3: espNowMode = inputMode;
if (InfoPrint == 1) {Serial.println("esp-now mode: follower");}
screenLine_3 = "ESP-NOW: > FOLLOWER <";
oled_update();
jsonInfoHttp.clear();
jsonInfoHttp["info"] = "esp-now mode: follower";
break;
}
}
// callback when data is sent
void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status) {
char macStr[18];
// Serial.print("Packet to: ");
// Copies the sender mac address to a string
snprintf(macStr, sizeof(macStr), "%02x:%02x:%02x:%02x:%02x:%02x",
mac_addr[0], mac_addr[1], mac_addr[2], mac_addr[3], mac_addr[4], mac_addr[5]);
jsonInfoHttp.clear();
jsonInfoHttp["T"] = CMD_ESP_NOW_SEND;
jsonInfoHttp["mac"] = macStr;
jsonInfoHttp["status"] = (status == ESP_NOW_SEND_SUCCESS ? 1 : 0);
jsonInfoHttp["megs"] = (status == ESP_NOW_SEND_SUCCESS ? "Delivery Success" : "Delivery Fail");
String getInfoJsonString;
serializeJson(jsonInfoHttp, getInfoJsonString);
Serial.println(getInfoJsonString);
}
void macStringToByteArray(const String& macString, uint8_t* byteArray) {
for (int i = 0; i < 6; i++) {
byteArray[i] = strtol(macString.substring(i * 3, i * 3 + 2).c_str(), NULL, 16);
}
return;
}
void OnDataRecv(const unsigned char* mac, const unsigned char* incomingData, int len) {
if (espNowMode != 3){
return;
}
memcpy(&espNowMegsRecv, incomingData, sizeof(espNowMegsRecv));
if (espNowMegsRecv.cmd == 3) {
char macStr[18];
snprintf(macStr, sizeof(macStr), "%02x:%02x:%02x:%02x:%02x:%02x",
mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
jsonInfoHttp.clear();
jsonInfoHttp["T"] = CMD_ESP_NOW_RECV;
jsonInfoHttp["mac"] = macStr;
jsonInfoHttp["megs"] = espNowMegsRecv.message;
String getInfoJsonString;
serializeJson(jsonInfoHttp, getInfoJsonString);
Serial.println(getInfoJsonString);
}
if (!ctrlByBroadcast) {
if (memcmp(mac, mac_whitelist_broadcast, 6) != 0) {
return;
}
}
// if (InfoPrint == 1) {
// Serial.print("Bytes received: ");Serial.println(len);
// }
switch(espNowMegsRecv.cmd) {
case 0: {
RoArmM2_allJointAbsCtrl(espNowMegsRecv.base,
espNowMegsRecv.shoulder,
espNowMegsRecv.elbow,
espNowMegsRecv.hand,
0,
0);break;
}
case 1: {
DeserializationError err = deserializeJson(jsonCmdReceive, espNowMegsRecv.message);
if (err == DeserializationError::Ok) {
jsonCmdReceiveHandler();
};break;
}
case 2: {
DeserializationError err = deserializeJson(jsonCmdReceive, espNowMegsRecv.message);
if (err == DeserializationError::Ok) {
runNewJsonCmd = true;
};break;
}
}
}
void initEspNow() {
if (esp_now_init() != ESP_OK) {
// Serial.println("Error initializing ESP-NOW");
jsonInfoHttp.clear();
jsonInfoHttp["T"] = CMD_ESP_NOW_SEND;
jsonInfoHttp["status"] = 2;
jsonInfoHttp["megs"] = "Error initializing ESP-NOW";
String getInfoJsonString;
serializeJson(jsonInfoHttp, getInfoJsonString);
Serial.println(getInfoJsonString);
return;
}
// register esp-now sending call back function.
esp_now_register_send_cb(OnDataSent);
// register esp-now receving call back function.
esp_now_register_recv_cb(OnDataRecv);
// register peer
peerInfo.channel = ESP_NOW_CHANNEL;
peerInfo.encrypt = ESP_NOW_ENCRYPT;
}
void registerNewFollowerToPeer(String inputMac) {
if (inputMac.length() != 17) {
// Serial.println("invalid MAC address format.");
jsonInfoHttp.clear();
jsonInfoHttp["T"] = CMD_ESP_NOW_SEND;
jsonInfoHttp["status"] = 3;
jsonInfoHttp["megs"] = "invalid MAC address format.";
String getInfoJsonString;
serializeJson(jsonInfoHttp, getInfoJsonString);
Serial.println(getInfoJsonString);
return;
}
uint8_t macArray[6];
macStringToByteArray(inputMac, macArray);
for (int i = 0; i < 6; i++) {
singleFollowerDev[i] = macArray[i];
}
memcpy(peerInfo.peer_addr, macArray, 6);
if (esp_now_add_peer(&peerInfo) != ESP_OK) {
// Serial.println("Failed to add peer: " + inputMac);
jsonInfoHttp.clear();
jsonInfoHttp["T"] = CMD_ESP_NOW_SEND;
jsonInfoHttp["status"] = 4;
jsonInfoHttp["megs"] = "Failed to add peer.";
jsonInfoHttp["mac"] = inputMac;
String getInfoJsonString;
serializeJson(jsonInfoHttp, getInfoJsonString);
Serial.println(getInfoJsonString);
return;
}
// Serial.println("add peer: " + inputMac);
jsonInfoHttp.clear();
jsonInfoHttp["T"] = CMD_ESP_NOW_SEND;
jsonInfoHttp["status"] = 5;
jsonInfoHttp["megs"] = "add peer.";
jsonInfoHttp["mac"] = inputMac;
String getInfoJsonString;
serializeJson(jsonInfoHttp, getInfoJsonString);
Serial.println(getInfoJsonString);
}
void deleteFollower(String inputMac) {
if (inputMac.length() != 17) {
// Serial.println("invalid MAC address format.");
jsonInfoHttp.clear();
jsonInfoHttp["T"] = CMD_ESP_NOW_SEND;
jsonInfoHttp["status"] = 3;
jsonInfoHttp["megs"] = "invalid MAC address format.";
String getInfoJsonString;
serializeJson(jsonInfoHttp, getInfoJsonString);
Serial.println(getInfoJsonString);
return;
}
uint8_t macArray[6];
macStringToByteArray(inputMac, macArray);
esp_now_del_peer(macArray);
jsonInfoHttp.clear();
jsonInfoHttp["T"] = CMD_ESP_NOW_SEND;
jsonInfoHttp["status"] = 6;
jsonInfoHttp["megs"] = "delete peer.";
jsonInfoHttp["mac"] = inputMac;
String getInfoJsonString;
serializeJson(jsonInfoHttp, getInfoJsonString);
Serial.println(getInfoJsonString);
// if (InfoPrint == 1) {
// Serial.println("delete peer: " + inputMac);
// }
}
void espNowGroupSend(byte devCodeIn, float bIn, float sIn, float eIn, float hIn, byte cmdIn, String messageIn) {
espNowMessage.devCode = devCodeIn;
espNowMessage.base = bIn;
espNowMessage.shoulder = sIn;
espNowMessage.elbow = eIn;
espNowMessage.hand = hIn;
espNowMessage.cmd = cmdIn;
strcpy(espNowMessage.message, messageIn.c_str());
esp_err_t result = esp_now_send(0, (uint8_t *) &espNowMessage, sizeof(struct_message));
// if (result == ESP_OK) {
// Serial.println("Sent with success");
// }
// else {
// Serial.println("Error sending the data");
// }
jsonInfoHttp.clear();
jsonInfoHttp["T"] = CMD_ESP_NOW_SEND;
jsonInfoHttp["status"] = (result == ESP_OK ? 8 : 7);
jsonInfoHttp["megs"] = (result == ESP_OK ? "sent with success." : "error sending the data.");
String getInfoJsonString;
serializeJson(jsonInfoHttp, getInfoJsonString);
Serial.println(getInfoJsonString);
}
void espNowSingleDevSend(String inputMac, byte devCodeIn, float bIn, float sIn, float eIn, float hIn, byte cmdIn, String messageIn){
if (inputMac.length() != 17) {
// Serial.println("invalid MAC address format.");
jsonInfoHttp.clear();
jsonInfoHttp["T"] = CMD_ESP_NOW_SEND;
jsonInfoHttp["status"] = 3;
jsonInfoHttp["megs"] = "invalid MAC address format.";
String getInfoJsonString;
serializeJson(jsonInfoHttp, getInfoJsonString);
Serial.println(getInfoJsonString);
return;
}
espNowMessage.devCode = devCodeIn;
espNowMessage.base = bIn;
espNowMessage.shoulder = sIn;
espNowMessage.elbow = eIn;
espNowMessage.hand = hIn;
espNowMessage.cmd = cmdIn;
// espNowMessage.message = messageIn;
strcpy(espNowMessage.message, messageIn.c_str());
uint8_t macArray[6];
macStringToByteArray(inputMac, macArray);
for (int i = 0; i < 6; i++) {
singleFollowerDev[i] = macArray[i];
}
esp_err_t result = esp_now_send(
macArray,
(uint8_t *) &espNowMessage,
sizeof(struct_message));
// if (result == ESP_OK) {
// Serial.println("Sent with success");
// }
// else {
// Serial.println("Error sending the data");
// }
jsonInfoHttp.clear();
jsonInfoHttp["T"] = CMD_ESP_NOW_SEND;
jsonInfoHttp["status"] = (result == ESP_OK ? 8 : 7);
jsonInfoHttp["megs"] = (result == ESP_OK ? "sent with success." : "error sending the data.");
String getInfoJsonString;
serializeJson(jsonInfoHttp, getInfoJsonString);
Serial.println(getInfoJsonString);
}
void espNowSingleDevFlowCtrl() {
espNowMessage.base = radB;
espNowMessage.shoulder = radS;
espNowMessage.elbow = radE;
espNowMessage.hand = radG;
esp_err_t result = esp_now_send(singleFollowerDev,
(uint8_t *) &espNowMessage,
sizeof(struct_message));
// if (result == ESP_OK) {
// Serial.println("Sent with success");
// }
// else {
// Serial.println("Error sending the data");
// }
jsonInfoHttp.clear();
jsonInfoHttp["T"] = CMD_ESP_NOW_SEND;
jsonInfoHttp["status"] = (result == ESP_OK ? 8 : 7);
jsonInfoHttp["megs"] = (result == ESP_OK ? "sent with success." : "error sending the data.");
String getInfoJsonString;
serializeJson(jsonInfoHttp, getInfoJsonString);
Serial.println(getInfoJsonString);
}
void espNowGroupDevsFlowCtrl() {
espNowMessage.base = radB;
espNowMessage.shoulder = radS;
espNowMessage.elbow = radE;
espNowMessage.hand = radG;
esp_err_t result = esp_now_send(0, (uint8_t *) &espNowMessage, sizeof(struct_message));
// if (result == ESP_OK) {
// Serial.println("Sent with success");
// }
// else {
// Serial.println("Error sending the data");
// }
jsonInfoHttp.clear();
jsonInfoHttp["T"] = CMD_ESP_NOW_SEND;
jsonInfoHttp["status"] = (result == ESP_OK ? 8 : 7);
jsonInfoHttp["megs"] = (result == ESP_OK ? "sent with success." : "error sending the data.");
String getInfoJsonString;
serializeJson(jsonInfoHttp, getInfoJsonString);
Serial.println(getInfoJsonString);
}
void changeBroadcastMode(bool inputMode, String inputMac) {
ctrlByBroadcast = inputMode;
jsonInfoHttp.clear();
jsonInfoHttp["mode"] = inputMode;
uint8_t macArray[6];
macStringToByteArray(inputMac, macArray);
for (int i = 0; i < 6; i++) {
mac_whitelist_broadcast[i] = macArray[i];
}
if (InfoPrint == 1) {
if (ctrlByBroadcast) {
Serial.println("it can be ctrl by esp-now broadcast cmd.");
jsonInfoHttp["info"] = "it can be ctrl by esp-now broadcast cmd.";
jsonInfoHttp["leader mac"] = inputMac;
} else {
Serial.println("it won't be ctrl by esp-now broadcast cmd.");
jsonInfoHttp["info"] = "it won't be ctrl by esp-now broadcast cmd, leader mac: "+inputMac;
}
}
}
files_ctrl.h
+319
View File
@@ -0,0 +1,319 @@
// funcs for editing the files in flash.
bool flashStatus = false;
// initialize littleFS for flash file system ctrl.
void initFS() {
if (!LittleFS.begin(true)){
if (InfoPrint == 1) {Serial.println("LittleFS mount failed.");}
flashStatus = false;
}
else {
if (InfoPrint == 1) {Serial.println("LittleFS mount succeed.");}
flashStatus = true;
}
}
// get the free space of flash.
uint32_t freeFlashSpace(){
size_t total = LittleFS.totalBytes();
size_t used = LittleFS.usedBytes();
uint32_t freeSpace = total - used;
if (InfoPrint == 1) {
Serial.print("totalBytes:\t");Serial.print(total);
Serial.println(" bytes");
Serial.print("free flash memory:\t");Serial.print(freeSpace);
Serial.println(" bytes");
}
jsonInfoHttp.clear();
jsonInfoHttp["info"] = "free flash space";
jsonInfoHttp["total"] = total;
jsonInfoHttp["free"] = freeSpace;
return freeSpace;
}
// scan all the files saved in flash.
void scanFlashContents() {
jsonInfoHttp.clear();
File root = LittleFS.open("/");
if (!root.isDirectory()) {
if (InfoPrint == 1) {
Serial.println("error: not a directory.");
jsonInfoHttp["info"] = "error: not a directory.";
return;
}
}
jsonInfoHttp["info"] = "reading files and the first line";
File file = root.openNextFile();
while (file) {
if (!file.isDirectory()) {
Serial.println(">>>---=== File Name and First line ===---<<<");
Serial.println("[file]: [" + String(file.name()) + "]");
Serial.println("[first line]:");
String line = file.readStringUntil('\n');
if (line) {
Serial.println(line);
jsonInfoHttp[file.name()] = line;
} else {
Serial.println("no content.");
jsonInfoHttp[file.name()] = "[null]";
}
file.close();
} else if (file.isDirectory()) {
if (file) {
Serial.println("Failed to open file: " + String(file.name()));
jsonInfoHttp[file.name()] = "[failed to open]";
}
}
file = root.openNextFile();
}
}
// create a new file and input the content.
bool createFile(String fileName, String fileContent) {
jsonInfoHttp.clear();
if (!flashStatus) {
if (InfoPrint == 1) {Serial.println("LittleFS mount failed.");}
jsonInfoHttp["info"] = "LittleFS mount failed.";
return false;
}
if (LittleFS.exists("/"+fileName)) {
if (InfoPrint == 1) {Serial.println("file already exists.");}
jsonInfoHttp["info"] = "file already exists.";
return false;
}
File file = LittleFS.open("/"+fileName, "w");
if (file) {
// file.println("{\"name\":\"" + fileName + "\",\"intro\":\"" + fileContent + "\"}");
file.println(fileContent);
file.close();
if (InfoPrint == 1) {Serial.println("file created successfully.");}
jsonInfoHttp["info"] = "file created successfully.";
return true;
} else {
if (InfoPrint == 1) {Serial.println("file creation failed.");}
jsonInfoHttp["info"] = "file creation failed.";
return false;
}
}
// read a file, this function return the lineNum.
int readFile(String fileName) {
jsonInfoHttp.clear();
jsonInfoHttp["info"] = "reading file";
File file = LittleFS.open("/" + fileName, "r");
if (!file) {
Serial.println("file not found.");
jsonInfoHttp["info"] = "file not found";
return -1;
}
Serial.println("---=== File Content ===---");
Serial.println("reading file: [" + fileName + "] starts:");
jsonInfoHttp["name"] = fileName;
int _LineNum = -1;
while (file.available()) {
_LineNum++;
String line = file.readStringUntil('\n');
Serial.print("[lineNum: ");Serial.print(_LineNum+1);Serial.print(" ] - ");
Serial.println(line);
jsonInfoHttp["lineNum_"+String(_LineNum+1)] = line;
}
Serial.println("^^^ ^^^ ^^^ reading file: " + fileName + " ends. ^^^ ^^^ ^^^");
file.close();
return _LineNum + 1;
}
// delete a file.
bool deleteFile(String inputName) {
jsonInfoHttp.clear();
if (!flashStatus) {
if (InfoPrint == 1) {Serial.println("LittleFS mount failed.");}
jsonInfoHttp["info"] = "LittleFS mount failed.";
return false;
}
if (!LittleFS.exists("/" + inputName)) {
if (InfoPrint == 1) {Serial.println("file already deleted.");}
jsonInfoHttp["info"] = "file already deleted.";
return false;
}
LittleFS.remove("/" + inputName);
if (InfoPrint == 1) {Serial.println("file deleted successfully.");}
jsonInfoHttp["info"] = "file deleted successfully.";
return true;
}
// add content at the end of a file.
void appendLine(String fileName, String appendContent) {
Serial.println("--- --- --- RAW FILE -- --- ---");
if (readFile(fileName) == -1) {return;}
File file = LittleFS.open("/" + fileName, "a");
if(!file){
Serial.println("Error opening file for appending.");
return;
}
file.println(appendContent);
file.close();
Serial.println("--- --- --- NEW FILE -- --- ---");
jsonInfoHttp.clear();
readFile(fileName);
}
// insert a new line under the lineNum.
void insertLine(String filename, int lineNum, String newLineString) {
Serial.println("--- --- --- RAW FILE -- --- ---");
int _LineNum = readFile(filename);
if (_LineNum == -1) {return;}
String lines[_LineNum+1];
File file = LittleFS.open("/" + filename, "r");
if(!file){
Serial.println("Error opening file for writing.");
return;
}
int i = 0;
while (file.available()) {
if (i == lineNum - 1) {
lines[i] = newLineString;
i++;
}
lines[i] = file.readStringUntil('\n');
i++;
}
file.close();
file = LittleFS.open("/" + filename, "w");
for(int j=0; j<i; j++){
file.println(lines[j]);
}
file.close();
Serial.println("--- --- --- NEW FILE -- --- ---");
jsonInfoHttp.clear();
readFile(filename);
}
// change a single line in the file.
void replaceLine(String filename, int lineNum, String newLineString) {
Serial.println("--- --- --- RAW FILE -- --- ---");
int _LineNum = readFile(filename);
if (_LineNum == -1) {return;}
String lines[_LineNum];
File file = LittleFS.open("/" + filename, "r");
if(!file){
Serial.println("Error opening file.");
return;
}
int i = 0;
while (file.available()) {
lines[i] = file.readStringUntil('\n');
i++;
}
file.close();
lines[lineNum-1] = newLineString;
file = LittleFS.open("/" + filename, "w");
for(int j=0; j<i; j++){
file.println(lines[j]);
}
file.close();
Serial.println("--- --- --- NEW FILE -- --- ---");
jsonInfoHttp.clear();
readFile(filename);
}
// read a single line from file.
String readSingleLine(String filename, int lineNum) {
File file = LittleFS.open("/" + filename, "r");
if(!file){
Serial.println("Error opening file.");
return "";
}
jsonInfoHttp.clear();
String line;
int i = 0;
while (file.available()) {
line = file.readStringUntil('\n');
if (i == lineNum-1) {
file.close();
if (InfoPrint == 1) {
Serial.println(line);
jsonInfoHttp["filename"] = filename;
jsonInfoHttp["lineNum"] = lineNum;
}
return line;
}
i++;
}
file.close();
if (InfoPrint == 1) {
Serial.println("[line not found]");
}
return "";
}
void deleteSingleLine(String fileName, int lineNum){
Serial.println("--- --- --- RAW FILE -- --- ---");
File file = LittleFS.open("/" + fileName, "r+");
readFile(fileName);
if(!file){
Serial.println("Error opening file for reading");
return;
}
String contents = "";
int i = 1;
while(file.available()){
String line = file.readStringUntil('\n');
if(i != lineNum){
contents += line + "\n";
}
i++;
}
file.close();
file = LittleFS.open("/" + fileName, "w");
file.print(contents);
file.close();
Serial.println("--- --- --- NEW FILE -- --- ---");
jsonInfoHttp.clear();
readFile(fileName);
}
gimbal_module.h
+198
View File
@@ -0,0 +1,198 @@
u8 gimbalID[2] = {GIMBAL_PAN_ID, GIMBAL_TILT_ID};
s16 gimbalPos[2];
u16 gimbalSpd[2];
u8 gimbalAcc[2];
ServoFeedback gimbalFeedback[2];
// [0] PAN
// [1] TILT
float steadyGoalY = 0;
float constrainFloat(float value, float min, float max) {
if (value < min) {
return min;
} else if (value > max) {
return max;
}
return value;
}
float mapFloat(float value, float fromLow, float fromHigh, float toLow, float toHigh) {
return (value - fromLow) * (toHigh - toLow) / (fromHigh - fromLow) + toLow;
}
void gimbalCtrlSimple(float Xinput, float Yinput, float spdInput, float accInput) {
Xinput = constrainFloat(Xinput, -180, 180);
Yinput = constrainFloat(Yinput, -30, 90);
gimbalPos[0] = 2047 + (int)round(map(Xinput, 0, 360, 0, 4095));
gimbalPos[1] = 2047 - (int)round(map(Yinput, 0, 360, 0, 4095));
gimbalSpd[0] = (int)round(map(spdInput, 0, 360, 0, 4095));
gimbalSpd[1] = (int)round(map(spdInput, 0, 360, 0, 4095));
gimbalAcc[0] = (int)round(map(accInput, 0, 360, 0, 4095));
gimbalAcc[1] = (int)round(map(accInput, 0, 360, 0, 4095));
st.SyncWritePosEx(gimbalID, 2, gimbalPos, gimbalSpd, gimbalAcc);
}
void gimbalCtrlMove(float Xinput, float Yinput, float spdInputX, float spdInputY) {
Xinput = constrainFloat(Xinput, -180, 180);
Yinput = constrainFloat(Yinput, -30, 90);
spdInputX = constrain(spdInputX, 1, 2500);
spdInputY = constrain(spdInputY, 1, 2500);
gimbalPos[0] = 2047 + (int)round(map(Xinput, 0, 360, 0, 4095));
gimbalPos[1] = 2047 - (int)round(map(Yinput, 0, 360, 0, 4095));
gimbalSpd[0] = spdInputX;
gimbalSpd[1] = spdInputY;
gimbalAcc[0] = 0;
gimbalAcc[1] = 0;
st.SyncWritePosEx(gimbalID, 2, gimbalPos, gimbalSpd, gimbalAcc);
}
//mapFloat(float value, float fromLow, float fromHigh, float toLow, float toHigh)
float panAngleCompute(int inputPos) {
return mapFloat((inputPos - 2047), 0, 4095, 0, 360);
}
float tiltAngleCompute(int inputPos) {
return mapFloat((2047 - inputPos), 0, 4095, 0, 360);
}
void gimbalCtrlStop() {
st.EnableTorque(GIMBAL_PAN_ID, 0);
st.EnableTorque(GIMBAL_TILT_ID, 0);
delay(SERVO_STOP_DELAY);
st.EnableTorque(GIMBAL_PAN_ID, 1);
st.EnableTorque(GIMBAL_TILT_ID, 1);
}
void getGimbalFeedback() {
if(st.FeedBack(GIMBAL_PAN_ID)!=-1) {
gimbalFeedback[0].status = true;
gimbalFeedback[0].pos = st.ReadPos(-1);
gimbalFeedback[0].speed = st.ReadSpeed(-1);
gimbalFeedback[0].load = st.ReadLoad(-1);
gimbalFeedback[0].voltage = st.ReadVoltage(-1);
gimbalFeedback[0].current = st.ReadCurrent(-1);
gimbalFeedback[0].temper = st.ReadTemper(-1);
gimbalFeedback[0].mode = st.ReadMode(GIMBAL_PAN_ID);
} else{
servoFeedback[0].status = false;
if(InfoPrint == 1){
jsonInfoHttp.clear();
jsonInfoHttp["T"] = 1005;
jsonInfoHttp["id"] = GIMBAL_PAN_ID;
jsonInfoHttp["status"] = 0;
String getInfoJsonString;
serializeJson(jsonInfoHttp, getInfoJsonString);
Serial.println(getInfoJsonString);
}
}
if(st.FeedBack(GIMBAL_TILT_ID)!=-1) {
gimbalFeedback[1].status = true;
gimbalFeedback[1].pos = st.ReadPos(-1);
gimbalFeedback[1].speed = st.ReadSpeed(-1);
gimbalFeedback[1].load = st.ReadLoad(-1);
gimbalFeedback[1].voltage = st.ReadVoltage(-1);
gimbalFeedback[1].current = st.ReadCurrent(-1);
gimbalFeedback[1].temper = st.ReadTemper(-1);
gimbalFeedback[1].mode = st.ReadMode(GIMBAL_TILT_ID);
} else{
servoFeedback[1].status = false;
if(InfoPrint == 1){
jsonInfoHttp.clear();
jsonInfoHttp["T"] = 1005;
jsonInfoHttp["id"] = GIMBAL_TILT_ID;
jsonInfoHttp["status"] = 0;
String getInfoJsonString;
serializeJson(jsonInfoHttp, getInfoJsonString);
Serial.println(getInfoJsonString);
}
}
}
void gimbalSteadySet(bool inputCmd, float inputY) {
steadyMode = inputCmd;
if (inputY < -45) {
inputY = -45;
} else if (inputY > 90) {
inputY = 90;
}
steadyGoalY = inputY;
}
void gimbalSteady(float inputBiasY) {
if (!steadyMode) {
return;
}
gimbalCtrlSimple(0, inputBiasY - icm_pitch, 0, 0);
}
void gimbalUserCtrl(int inputX, int inputY, int inputSpd) {
static float goalX = 0;
static float goalY = 0;
if(inputX == -1 && inputY == 1){
goalX = -180;
goalY = 90;
}
else if(inputX == 0 && inputY == 1){
goalY = 90;
}
else if(inputX == 1 && inputY == 1){
goalX = 180;
goalY = 90;
}
else if(inputX == -1 && inputY == 0){
goalX = -180;
}
else if(inputX == 1 && inputY == 0){
goalX = 180;
}
else if(inputX == -1 && inputY == -1){
goalX = -180;
goalY = -45;
}
else if(inputX == 0 && inputY == -1){
goalY = -45;
}
else if(inputX == 1 && inputY == -1){
goalX = 180;
goalY = -45;
}
if(inputX == 2 && inputY == 2){
gimbalCtrlSimple(0, 0, 0, 10);
}
else{
gimbalCtrlSimple(goalX, goalY, inputSpd, 0);
if(inputX == 0){
servoTorqueCtrl(GIMBAL_PAN_ID, 0);
delay(5);
servoTorqueCtrl(GIMBAL_PAN_ID, 1);
getGimbalFeedback();
goalX = panAngleCompute(gimbalFeedback[0].pos);
}
if(inputY == 0){
servoTorqueCtrl(GIMBAL_TILT_ID, 0);
delay(5);
servoTorqueCtrl(GIMBAL_TILT_ID, 1);
getGimbalFeedback();
goalY = tiltAngleCompute(gimbalFeedback[1].pos);
}
}
}
http_server.h
+31
View File
@@ -0,0 +1,31 @@
#include "web_page.h"
// Create AsyncWebServer object on port 80
WebServer server(80);
void handleRoot(){
server.send(200, "text/html", index_html); //Send web page
}
void webCtrlServer(){
server.on("/", handleRoot);
server.on("/js", [](){
String jsonCmdWebString = server.arg(0);
deserializeJson(jsonCmdReceive, jsonCmdWebString);
jsonCmdReceiveHandler();
serializeJson(jsonInfoHttp, jsonFeedbackWeb);
server.send(200, "text/plane", jsonFeedbackWeb);
jsonFeedbackWeb = "";
jsonInfoHttp.clear();
jsonCmdReceive.clear();
});
// Start server
server.begin();
Serial.println("Server Starts.");
}
void initHttpWebServer(){
webCtrlServer();
}
json_cmd.h
+575
View File
@@ -0,0 +1,575 @@
#define FEEDBACK_BASE_INFO 1001
#define FEEDBACK_IMU_DATA 1002
// esp-now recv
// {"T":1003,"mac":"FF:FF:FF:FF:FF:FF","megs":"hello!"}
#define CMD_ESP_NOW_RECV 1003
// esp-now send status
// 0:failed 1:succeed 2:Error initializing ESP-NOW
// 3:invalid MAC address format.
// 4:Failed to add peer.
// 5:add peer. 6:delete peer.
// 7:error sending the data. 8:sent with success.
// {"T":1004,"mac":"FF:FF:FF:FF:FF:FF","status":1,"megs":"xxx"}
#define CMD_ESP_NOW_SEND 1004
// bus servos error feedback
// {"T":1005,"id":1,"status":1}
#define CMD_BUS_SERVO_ERROR 1005
// ---===< EoAT type settings. >===---
// modeType 0: gripper
// {"T":124,"mode":0}
// modeType 1: wrist
// {"T":124,"mode":1}
#define CMD_EOAT_TYPE 124
// EoAT assemble.
// mount position: 0 - edge
// 1 - D-3.2
// 2 - D-4.2
// 3 - D-10.2
// -------L3A-----------O==L2B===
// | ^ ||
// L3B | ||
// | ELBOW_JOINT ||
// pos->X--L4A---O L2A
// | ||
// | L4B ||
// / | ||
// PI X-EA-X SHOULDER_JOINT -> OO
// \ | [||]
// EB L1
// | [||]
// -------- BASE_JOINT -> XX
// unit:mm
// {"T":125,"pos":3,"ea":0,"eb":20}
#define CMD_CONFIG_EOAT 125
// ---===< UGV ctrl. >===---
// SPEED_INPUT
// {"T":1,"L":0.5,"R":0.5}
#define CMD_SPEED_CTRL 1
// {"T":11,"L":164,"R":164} (input PWM +-255)
#define CMD_PWM_INPUT 11
// {"T":13,"X":0.1,"Z":0.3} (m/s,rad/s)(Not for the products without encoders)
#define CMD_ROS_CTRL 13
// MOTOR PID & WINDUP LIMITS
// {"T":2,"P":200,"I":2500,"D":0,"L":255}
// {"T":2,"P":20,"I":2500,"D":0,"L":255}
// {"T":222,"name":"mission_a","step":"{\"T\":104,\"x\":235,\"y\":0,\"z\":234,\"t\":3.14,\"spd\":0.25}"}
// {"T":222,"name":"boot","step":"{\"T\":2,\"P\":20,\"I\":2500,\"D\":0,\"L\":255}"}
#define CMD_SET_MOTOR_PID 2
// OLED INFO SET
// {"T":3,"lineNum":0,"Text":"putYourTextHere"}
#define CMD_OLED_CTRL 3
// OLED DEFAULT
// {"T":-3}
#define CMD_OLED_DEFAULT -3
// MODULE TYPE
// 0: nothing
// 1: RoArm-M2-S
// 2: Gimbal
// {"T":4,"cmd":0}
#define CMD_MODULE_TYPE 4
// {"T":126}
#define CMD_GET_IMU_DATA 126
// the robot need to be put on a ground and kept still
// getting the imu offset and set as default
// this gonna take a while (5s)
// {"T":127}
#define CMD_CALI_IMU_STEP 127
// {"T":128}
#define CMD_GET_IMU_OFFSET 128
// {"T":129,"x":-12,"y":0,"z":0}
#define CMD_SET_IMU_OFFSET 129
// {"T":130}
#define CMD_BASE_FEEDBACK 130
// off: {"T":131,"cmd":0} [default]
// on: {"T":131,"cmd":1}
#define CMD_BASE_FEEDBACK_FLOW 131
// set the extra delay time(ms) for feedback info
// {"T":142,"cmd":0}
#define CMD_FEEDBACK_FLOW_INTERVAL 142 // dev
// set the echo mode of recving new cmd.
// 0: [default]off
// 1: on
// {"T":143,"cmd":0}
#define CMD_UART_ECHO_MODE 143
// LIGHT/GIMBAL/MOVTION CTRL
// {"T":132,"IO4":255,"IO5":255}
#define CMD_LED_CTRL 132
// GIMBAL CTRL(SIMPLE)
// {"T":133,"X":45,"Y":45,"SPD":0,"ACC":0}
#define CMD_GIMBAL_CTRL_SIMPLE 133
// GIMBAL CTRL MOVE
// {"T":134,"X":45,"Y":45,"SX":300,"SY":300}
#define CMD_GIMBAL_CTRL_MOVE 134
// GIMBAL CTRL STOP
// {"T":135}
#define CMD_GIMBAL_CTRL_STOP 135
// CHANGE HEART BEAT DELAY
// {"T":136,"cmd":3000}
#define CMD_HEART_BEAT_SET 136
// GIMBAL STEADY
// off: {"T":137,"s":0,"y":0}
// on: {"T":137,"s":1,"y":0}
#define CMD_GIMBAL_STEADY 137
// SET SPEED RATE
// {"T":138,"L":1,"R":1}
#define CMD_SET_SPD_RATE 138
// GET SPEED RATE
// {"T":139}
#define CMD_GET_SPD_RATE 139
// SAVE SPEED RATE
// {"T":140}
#define CMD_SAVE_SPD_RATE 140
// GIMBAL USER CTRL
// {"T":141,"X":0,"Y":0,"SPD":300}
// -1: decrease
// 1: increase
// 0: stop
// 2,2: middle
#define CMD_GIMBAL_USER_CTRL 141
// ---===< Arm ctrl. >===---
// it moves to goal position directly.
// without interpolation.
// {"T":100}
#define CMD_MOVE_INIT 100
// {"T":101,"joint":0,"rad":0,"spd":0,"acc":10}
// joint: 1-BASE_JOINT + ->left
// 2-SHOULDER_JOINT + ->down
// 3-ELBOW_JOINT + ->down
// 4-EOAT_JOINT + ->grab/down
// spd: steps/s
// acc: steps/s^2
#define CMD_SINGLE_JOINT_CTRL 101
// {"T":102,"base":0,"shoulder":0,"elbow":1.57,"hand":1.57,"spd":0,"acc":10}
// input the angle in rad(180°=3.1415926).
#define CMD_JOINTS_RAD_CTRL 102
// {"T":103,"axis":2,"pos":0,"spd":0.25}
// axis: 1-x: 235.11
// 2-y: 0
// 3-z: 234.79
// 4-t: 1.57
#define CMD_SINGLE_AXIS_CTRL 103
// // // // // // // // // // // // // // // // // // // // //
// {"T":104,"x":235,"y":0,"z":234,"t":3.14,"spd":0.25} //
#define CMD_XYZT_GOAL_CTRL 104 //
// // // // // // // // // // // // // // // // // // // // //
// {"T":1041,"x":235,"y":0,"z":234,"t":3.14}
#define CMD_XYZT_DIRECT_CTRL 1041
// {"T":105}
// x: real x position.
// y: real y position.
// z: real z position.
// t: real grab/hand angle in rad.
// torB: base joint torque.
// torS: shoulder joint torque.
// torE: elbow joint torque.
#define CMD_SERVO_RAD_FEEDBACK 105
// release:
// {"T":106,"cmd":1.57,"spd":0,"acc":0}
// grab:
// {"T":106,"cmd":3.14,"spd":0,"acc":0}
// hand joint ctrl using angle in radius.
// {"T":106,"cmd":4.0,"spd":0,"acc":0}
#define CMD_EOAT_HAND_CTRL 106
// {"T":107,"tor":200}
#define CMD_EOAT_GRAB_TORQUE 107
// {"T":108,"joint":3,"p":16,"i":0}
// change the P&I of a joint.
// BASE_JOINT - 1
// SHOULDER_JOINT - 2
// ELBOW_JOINT - 3
// EOAT_JOINT - 4
// default p:32[servo] 16[RoArm-M2]
// i: 0[servo] 8[RoArm-M2 PID MODE ON]
// d: not used by default
#define CMD_SET_JOINT_PID 108
// {"T":109}
// reset the PID.
#define CMD_RESET_PID 109
// set a new x-axis.
// {"T":110,"xAxisAngle":0}
#define CMD_SET_NEW_X 110
// set delay time
// {"T":111,"cmd":3000}
#define CMD_DELAY_MILLIS 111
// dynamic external force adaptation.
// mode: 0 - stop: reset every limit torque to 1000.
// 1 - start: set the joint limit torque.
// b, s, e, h = bassJoint, shoulderJoint, elbowJoint, handJoint
// example:
// starts. input the limit torque of every joint.
// {"T":112,"mode":1,"b":60,"s":110,"e":50,"h":50}
// stop
// {"T":112,"mode":0,"b":1000,"s":1000,"e":1000,"h":1000}
#define CMD_DYNAMIC_ADAPTATION 112
// switch-12V ctrl.(NOT FOR UVG)
// pwm: -255 ~ 0(off) ~ +255
// {"T":113,"pwm_a":-255,"pwm_b":-255}
#define CMD_SWITCH_CTRL 113
// light ctrl.(NOT FOR UVG)
// led: 0(off) - 255(max)
// {"T":114,"led":255}
#define CMD_LIGHT_CTRL 114
// switch off.
// {"T":115}
#define CMD_SWITCH_OFF 115
// ctrl a single joint abs angle in deg.
// joint: 1-BASE_JOINT + ->left
// 2-SHOULDER_JOINT + ->down
// 3-ELBOW_JOINT + ->down
// 4-EOAT_JOINT + ->grab/down
// spd: speed, angle/s^2
// acc: speed, angle/s^2 (max: 22.5)
// {"T":121,"joint":1,"angle":0,"spd":10,"acc":10}
#define CMD_SINGLE_JOINT_ANGLE 121
// ctrl all joints
// b - BASE
// s - SHOULDER
// e - ELBOW
// h - HAND
// spd - angle/s
// acc - angle/s^2 (max: 22.5)
// {"T":122,"b":0,"s":0,"e":90,"h":180,"spd":10,"acc":10}
#define CMD_JOINTS_ANGLE_CTRL 122
// constant ctrl
// m: 0 - angle
// 1 - xyzt
// cmd: 0 - stop
// 1 - increase
// 2 - decrease
// {"T":123,"m":0,"axis":0,"cmd":0,"spd":3}
#define CMD_CONSTANT_CTRL 123
// 124/125...131
// === === === MISSION CTRL & FILE CTRL === === ===
// scan files in flash.
// {"T":200}
#define CMD_SCAN_FILES 200
// create a new file and input the content.
// {"T":201,"name":"file.txt","content":"inputContentHere."}
#define CMD_CREATE_FILE 201
// get a file content.
// {"T":202,"name":"file.txt"}
#define CMD_READ_FILE 202
// remove a file in flash.
// {"T":203,"name":"file.txt"}
#define CMD_DELETE_FILE 203
// add a line at the end of a file.
// {"T":204,"name":"file.txt","content":"inputContentHere."}
#define CMD_APPEND_LINE 204
// insert a new line as lineNum.
// {"T":205,"name":"file.txt","lineNum":3,"content":"content"}
#define CMD_INSERT_LINE 205
// change a single line in the file.
// {"T":206,"name":"file.txt","lineNum":3,"content":"Content"}
#define CMD_REPLACE_LINE 206
// read a single line from file.
// {"T":207,"name":"file.txt","lineNum":3}
#define CMD_READ_LINE 207
// delete a single line from file.
// {"T":208,"name":"file.txt","lineNum":3}
#define CMD_DELETE_LINE 208
// torque-lock ctrl.
// off: {"T":210,"cmd":0}
// on: {"T":210,"cmd":1}
#define CMD_TORQUE_CTRL 210
// === === === mission & steps edit. === === ===
// create a mission in flash:
// {"T":220,"name":"mission_a","intro":"test mission created in flash."}
#define CMD_CREATE_MISSION 220
// input the mission name and get the total content.
// {"T":221,"name":"mission_a"}
#define CMD_MISSION_CONTENT 221
// {"T":144,"E":100,"Z":0,"R":0}
#define CMD_ARM_CTRL_UI 144
// append a new step at the end of the mission, using the step input.
// {"T":222,"name":"mission_a","step":"{\"T\":104,\"x\":235,\"y\":0,\"z\":234,\"t\":3.14,\"spd\":0.25}"}
#define CMD_APPEND_STEP_JSON 222
// append a new step at the end of the mission, using the feedback.
// {"T":223,"name":"mission_a","spd":0.25}
#define CMD_APPEND_STEP_FB 223
// append a new delay(ms) at the end of the mission.
// {"T":224,"name":"mission_a","delay":3000}
#define CMD_APPEND_DELAY 224
// insert a new step as the stepNum
// using the json string input.
// {"T":225,"name":"mission_a","stepNum":3,"step":"{\"T\":104,\"x\":235,\"y\":0,\"z\":234,\"t\":3.14,\"spd\":0.25}"}
// {"T":225,"name":"mission_a","stepNum":3,"step":"{\"T\":114,\"led\":255}"}
#define CMD_INSERT_STEP_JSON 225
// insert a new step as the stepNum
// using the feedback.
// {"T":226,"name":"mission_a","stepNum":3,"spd":0.25}
#define CMD_INSERT_STEP_FB 226
// insert a new delay(ms) at the stepNum.
// {"T":227,"stepNum":3,"delay":3000}
#define CMD_INSERT_DELAY 227
// replace the cmd at stepNum
// using json cmd input.
// {"T":228,"name":"mission_a","stepNum":3,"step":"{\"T\":114,\"led\":255}"}
#define CMD_REPLACE_STEP_JSON 228
// replace the cmd at stepNum
// using feedback.
// {"T":229,"name":"mission_a","stepNum":3,"spd":0.25}
#define CMD_REPLACE_STEP_FB 229
// replace the cmd at stepNum with delay cmd.
// {"T":230,"name":"mission_a","stepNum":3,"delay":3000}
#define CMD_REPLACE_DELAY 230
// delete a step
// {"T":231,"name":"mission_a","stepNum":3}
#define CMD_DELETE_STEP 231
// input the mission name and a stepNum, it will move to the step.
// {"T":241,"name":"mission_a","stepNum":3}
#define CMD_MOVE_TO_STEP 241
// input the mission name and repeatTimes to play a mission.
// if repeatTimes = -1, it will loop forever.
// {"T":242,"name":"mission_a","times":3}
#define CMD_MISSION_PLAY 242
// === === === ESP-NOW settings. === === ===
// note: wifi must be running under STA(AP+STA) mode.
// it will be controled by broadcast mac address.
// {"T":300,"mode":1} [default]
// it won't be controled by broadcast mac address, and add one mac to whitelist.
// if there is no leader you can just fill 00:00:00:00:00:00 in it.
// {"T":300,"mode":0,"mac":"CC:DB:A7:5B:E4:1C"}
#define CMD_BROADCAST_FOLLOWER 300
// set the mode of esp-now
// espNowMode: 0 - none
// 1 - flow-leader(group): sending cmds
// 2 - flow-leader(single): sending cmds to a single follower
// 3 - [default]follower: recv cmds
// flow-leader - use cmd=0, ctrl servos in real time.
// leader uses the servos feedback pos to ctrl followers.
// {"T":301,"mode":3}
#define CMD_ESP_NOW_CONFIG 301
// get this dev mac address.
// {"T":302}
#define CMD_GET_MAC_ADDRESS 302
// add a new follower mac address to peer.
// {"T":303,"mac":"FF:FF:FF:FF:FF:FF"}
// {"T":303,"mac":"CC:DB:A7:5B:E4:1C"}
// {"T":303,"mac":"CC:DB:A7:5C:1C:40"}
// {"T":303,"mac":"CC:DB:A7:5C:E5:FC"}
#define CMD_ESP_NOW_ADD_FOLLOWER 303
// remove a follower from peer.
// {"T":304,"mac":"FF:FF:FF:FF:FF:FF"}
// {"T":304,"mac":"CC:DB:A7:5B:E4:1C"}
// {"T":304,"mac":"CC:DB:A7:5C:1C:40"}
// {"T":304,"mac":"CC:DB:A7:5C:E5:FC"}
#define CMD_ESP_NOW_REMOVE_FOLLOWER 304
// send info to more than one peer devs.
// "FF:FF:FF:FF:FF:FF" can't be in the broadcast peer.
// {"T":305,"dev":0,"b":0,"s":0,"e":1.57,"h":1.57,"cmd":0,"megs":"hello!"}
#define CMD_ESP_NOW_GROUP_CTRL 305
// send info to a single dev, or to every devs by using "FF:FF:FF:FF:FF:FF"
// broadcast ctrl:
// {"T":306,"mac":"FF:FF:FF:FF:FF:FF","dev":0,"b":0,"s":0,"e":1.57,"h":1.57,"cmd":0,"megs":"hello!"}
// {"T":306,"mac":"FF:FF:FF:FF:FF:FF","dev":0,"b":0,"s":0,"e":0,"h":0,"cmd":1,"megs":"{\"T\":114,\"led\":255}"}
// single ctrl:
// {"T":306,"mac":"CC:DB:A7:5C:E5:FC","dev":0,"b":0,"s":0,"e":1.57,"h":1.57,"cmd":0,"megs":"hello!"}
#define CMD_ESP_NOW_SINGLE 306
// === === === wifi settings. === === ===
// config the wifi mode on boot.
// 0 - off
// 1 - ap
// 2 - sta
// 3 - ap+sta
// {"T":401,"cmd":3}
#define CMD_WIFI_ON_BOOT 401
// config ap mode.
// {"T":402,"ssid":"UGV","password":"12345678"}
#define CMD_SET_AP 402
// config sta mode.
// {"T":403,"ssid":"na","password":"ps"}
#define CMD_SET_STA 403
// config ap/sta mode.
// {"T":404,"ap_ssid":"UGV","ap_password":"12345678","sta_ssid":"na","sta_password":"ps"}
#define CMD_WIFI_APSTA 404
// get wifi info.
// {"T":405}
#define CMD_WIFI_INFO 405
// create a wifiConfig.json file
// from the args already be using.
// {"T":406}
#define CMD_WIFI_CONFIG_CREATE_BY_STATUS 406
// create a wifiConfig.json file
// from the args input.
// {"T":407,"mode":3,"ap_ssid":"UGV","ap_password":"12345678","sta_ssid":"na","sta_password":"ps"}
#define CMD_WIFI_CONFIG_CREATE_BY_INPUT 407
// disconnect wifi.
// {"T":408}
#define CMD_WIFI_STOP 408
// === === === servo settings. === === ===
// change a servo's ID.
// {"T":501,"raw":1,"new":11}
#define CMD_SET_SERVO_ID 501
// set the current position as the middle position.
// > BASE_SERVO_ID 11
// > SHOULDER_DRIVING_SERVO_ID 12
// > SHOULDER_DRIVEN_SERVO_ID 13
// > ELBOW_SERVO_ID 14
// > GRIPPER_SERVO_ID 15
// {"T":502,"id":11}
#define CMD_SET_MIDDLE 502
// set the P/PID of a single servo.
// {"T":503,"id":14,"p":16}
#define CMD_SET_SERVO_PID 503
// === === === esp32 settings. === === ===
// esp-32 ctrl.
// reboot device.
// {"T":600}
#define CMD_REBOOT 600
// get the size of free flash space
// {"T":601}
#define CMD_FREE_FLASH_SPACE 601
// boot mission info.
// {"T":602}
#define CMD_BOOT_MISSION_INFO 602
// reset boot mission.
// {"T":603}
#define CMD_RESET_BOOT_MISSION 603
// if there is something wrong with wifi funcs, clear the nvs.
// {"T":604}
#define CMD_NVS_CLEAR 604
// 2: flow feedback.
// 1: [default]print debug info in serial.
// 0: don't print debug info in serial.
// {"T":605,"cmd":1}
#define CMD_INFO_PRINT 605
// === === === mainType & moduleType settings. === === ===
// {"T":900,"main":1,"module":0}
// main_type: 1-WAVE ROVER, 2-UGV02, 3-UGV01
#define CMD_MM_TYPE_SET 900
movtion_module.h
+449
View File
@@ -0,0 +1,449 @@
// switch parts
int switch_pwm_A = 0;
int switch_pwm_B = 0;
bool usePIDCompute = false;
float spd_rate_A = 1.0;
float spd_rate_B = 1.0;
bool heartbeatStopFlag = false;
void movtionPinInit(){
pinMode(AIN1, OUTPUT);
pinMode(AIN2, OUTPUT);
pinMode(PWMA, OUTPUT);
pinMode(BIN1, OUTPUT);
pinMode(BIN2, OUTPUT);
pinMode(PWMB, OUTPUT);
ledcSetup(channel_A, freq, ANALOG_WRITE_BITS);
ledcAttachPin(PWMA, channel_A);
ledcSetup(channel_B, freq, ANALOG_WRITE_BITS);
ledcAttachPin(PWMB, channel_B);
digitalWrite(AIN1, LOW);
digitalWrite(AIN2, LOW);
digitalWrite(BIN1, LOW);
digitalWrite(BIN2, LOW);
}
void switchEmergencyStop(){
digitalWrite(AIN1, LOW);
digitalWrite(AIN2, LOW);
digitalWrite(BIN1, LOW);
digitalWrite(BIN2, LOW);
}
void switchPortCtrlA(float pwmInputA){
int pwmIntA = round(pwmInputA * spd_rate_A);
if(abs(pwmIntA) < 1e-6){
digitalWrite(AIN1, LOW);
digitalWrite(AIN2, LOW);
return;
}
if(pwmIntA > 0){
digitalWrite(AIN1, LOW);
digitalWrite(AIN2, HIGH);
ledcWrite(channel_A, pwmIntA);
}
else{
digitalWrite(AIN1, HIGH);
digitalWrite(AIN2, LOW);
ledcWrite(channel_A,-pwmIntA);
}
}
void switchPortCtrlB(float pwmInputB){
int pwmIntB = round(pwmInputB * spd_rate_B);
if(abs(pwmIntB) < 1e-6){
digitalWrite(BIN1, LOW);
digitalWrite(BIN2, LOW);
return;
}
if(pwmIntB > 0){
digitalWrite(BIN1, LOW);
digitalWrite(BIN2, HIGH);
ledcWrite(channel_B, pwmIntB);
}
else{
digitalWrite(BIN1, HIGH);
digitalWrite(BIN2, LOW);
ledcWrite(channel_B,-pwmIntB);
}
}
void switchCtrl(int pwmIntA, int pwmIntB) {
switch_pwm_A = pwmIntA;
switch_pwm_B = pwmIntB;
switchPortCtrlA(switch_pwm_A);
switchPortCtrlB(switch_pwm_B);
}
void lightCtrl(int pwmIn) {
switch_pwm_A = pwmIn;
switchPortCtrlA(-abs(switch_pwm_A));
}
void setSpdRate(float inputL, float inputR) {
inputL = abs(inputL);
if (inputL > 1) {
inputL = 1;
}
inputR = abs(inputR);
if (inputR > 1) {
inputR = 1;
}
spd_rate_A = inputL;
spd_rate_B = inputR;
}
void getSpdRate() {
jsonInfoHttp.clear();
jsonInfoHttp["T"] = CMD_GET_SPD_RATE;
jsonInfoHttp["L"] = spd_rate_A;
jsonInfoHttp["R"] = spd_rate_B;
String getInfoJsonString;
serializeJson(jsonInfoHttp, getInfoJsonString);
Serial.println(getInfoJsonString);
}
// movtion parts.
// A-left, B-right
ESP32Encoder encoderA;
ESP32Encoder encoderB;
static unsigned long lastTime = 0;
static unsigned long lastLeftSpdTime = 0;
static unsigned long lastRightSpdTime = 0;
int lastEncoderA = 0;
int lastEncoderB = 0;
double speedGetA;
double speedGetB;
double plusesRate = 3.14159265359 * WHEEL_D / ONE_CIRCLE_PLUSES;
void initEncoders() {
// if(SET_MOTOR_DIR){
// encoderA.attachHalfQuad(AENCB, AENCA);
// encoderB.attachHalfQuad(BENCB, BENCA);
// }else{
encoderA.attachHalfQuad(AENCA, AENCB);
encoderB.attachHalfQuad(BENCA, BENCB);
// }
encoderA.setCount(0);
encoderB.setCount(0);
}
void getWheelSpeed() {
unsigned long currentTime = micros();
long encoderPulsesA = encoderA.getCount();
long encoderPulsesB = encoderB.getCount();
if (!SET_MOTOR_DIR) {
speedGetA = (plusesRate * (encoderPulsesA - lastEncoderA)) / ((double)(currentTime - lastTime) / 1000000);
speedGetB = (plusesRate * (encoderPulsesB - lastEncoderB)) / ((double)(currentTime - lastTime) / 1000000);
} else {
speedGetA = (plusesRate * (lastEncoderA - encoderPulsesA)) / ((double)(currentTime - lastTime) / 1000000);
speedGetB = (plusesRate * (lastEncoderB - encoderPulsesB)) / ((double)(currentTime - lastTime) / 1000000);
}
lastEncoderA = encoderPulsesA;
lastEncoderB = encoderPulsesB;
lastTime = currentTime;
}
void getLeftSpeed() {
unsigned long currentTime = micros();
long encoderPulsesA = encoderA.getCount();
if (!SET_MOTOR_DIR) {
speedGetA = (plusesRate * (encoderPulsesA - lastEncoderA)) / ((double)(currentTime - lastLeftSpdTime) / 1000000);
} else {
speedGetA = (plusesRate * (lastEncoderA - encoderPulsesA)) / ((double)(currentTime - lastLeftSpdTime) / 1000000);
}
lastEncoderA = encoderPulsesA;
lastLeftSpdTime = currentTime;
}
void getRightSpeed() {
unsigned long currentTime = micros();
long encoderPulsesB = encoderB.getCount();
if (!SET_MOTOR_DIR) {
speedGetB = (plusesRate * (encoderPulsesB - lastEncoderB)) / ((double)(currentTime - lastRightSpdTime) / 1000000);
} else {
speedGetB = (plusesRate * (lastEncoderB - encoderPulsesB)) / ((double)(currentTime - lastRightSpdTime) / 1000000);
}
lastEncoderB = encoderPulsesB;
lastRightSpdTime = currentTime;
}
// --- PID Controller ---
PID_v2 pidA(__kp, __ki, __kd, PID::Direct);
PID_v2 pidB(__kp, __ki, __kd, PID::Direct);
double outputA = 0;
double outputB = 0;
double setpointA = 0;
double setpointB = 0;
int setpoint_interval = 200;
unsigned long setpoint_cmd_recv = millis();
unsigned long setpoint_last_time = millis();
float setpointA_buffer;
float setpointB_buffer;
float setpointA_last;
float setpointB_last;
float change_offset = 0.005;
bool new_setpoint_flag = false;
void pidControllerInit() {
pidA.Start(speedGetA,
outputA,
setpointA);
pidA.SetOutputLimits(-255, 255);
pidA.SetMode(PID::Automatic);
pidB.Start(speedGetB,
outputB,
setpointB);
pidB.SetOutputLimits(-255, 255);
pidB.SetMode(PID::Automatic);
}
void leftCtrl(float pwmInputA){
int pwmIntA = round(pwmInputA);
if (mainType != 3) {
speedGetA = pwmIntA;
}
if(SET_MOTOR_DIR){
if(pwmIntA < 0){
digitalWrite(AIN1, HIGH);
digitalWrite(AIN2, LOW);
ledcWrite(channel_A, abs(pwmIntA));
}
else{
digitalWrite(AIN1, LOW);
digitalWrite(AIN2, HIGH);
ledcWrite(channel_A, abs(pwmIntA));
}
}else{
if(pwmIntA < 0){
digitalWrite(AIN1, LOW);
digitalWrite(AIN2, HIGH);
ledcWrite(channel_A, abs(pwmIntA));
}
else{
digitalWrite(AIN1, HIGH);
digitalWrite(AIN2, LOW);
ledcWrite(channel_A, abs(pwmIntA));
}
}
}
void rightCtrl(float pwmInputB){
int pwmIntB = round(pwmInputB);
if (mainType != 3) {
speedGetB = pwmIntB;
}
if(SET_MOTOR_DIR){
if(pwmIntB < 0){
digitalWrite(BIN1, HIGH);
digitalWrite(BIN2, LOW);
ledcWrite(channel_B, abs(pwmIntB));
}
else{
digitalWrite(BIN1, LOW);
digitalWrite(BIN2, HIGH);
ledcWrite(channel_B, abs(pwmIntB));
}
}else{
if(pwmIntB < 0){
digitalWrite(BIN1, LOW);
digitalWrite(BIN2, HIGH);
ledcWrite(channel_B, abs(pwmIntB));
}
else{
digitalWrite(BIN1, HIGH);
digitalWrite(BIN2, LOW);
ledcWrite(channel_B, abs(pwmIntB));
}
}
}
void setGoalSpeed(float inputLeft, float inputRight) {
// setpoint_cmd_recv = millis();
if (mainType == 3) {
usePIDCompute = true;
if(inputLeft < -2.0 || inputLeft > 2.0){
return;
}
if(inputRight < -2.0 || inputRight > 2.0){
return;
}
setpointA = inputLeft*spd_rate_A;
setpointB = inputRight*spd_rate_B;
if (setpointA != setpointA_buffer) {
pidA.Setpoint(setpointA);
setpointA_buffer = inputLeft;
}
if (setpointB != setpointB_buffer) {
pidB.Setpoint(setpointB);
setpointB_buffer = inputRight;
}
} else {
usePIDCompute = false;
leftCtrl(inputLeft * 512 * spd_rate_A);
rightCtrl(inputRight * 512 * spd_rate_B);
}
}
void pidControllerCompute() {
if (!usePIDCompute) {
return;
}
outputA = pidA.Run(speedGetA);
if (abs(outputA)<THRESHOLD_PWM) {
outputA = 0;
}
if (setpointA == 0 && speedGetA == 0) {
outputA = 0;
}
leftCtrl(outputA);
outputB = pidB.Run(speedGetB);
if (abs(outputB)<THRESHOLD_PWM) {
outputB = 0;
}
if (setpointB == 0 && speedGetB == 0) {
outputB = 0;
}
rightCtrl(outputB);
}
void LeftPidControllerCompute() {
if (!usePIDCompute) {
return;
}
outputA = pidA.Run(speedGetA);
if (abs(outputA)<THRESHOLD_PWM) {
outputA = 0;
}
if (setpointA == 0 && speedGetA == 0) {
outputA = 0;
}
leftCtrl(outputA);
}
void RightPidControllerCompute() {
if (!usePIDCompute) {
return;
}
outputB = pidB.Run(speedGetB);
if (abs(outputB)<THRESHOLD_PWM) {
outputB = 0;
}
if (setpointB == 0 && speedGetB == 0) {
outputB = 0;
}
rightCtrl(outputB);
}
void setPID(float inputP, float inputI, float inputD, float inputLimits) {
__kp = inputP;
__ki = inputI;
__kd = inputD;
windup_limits = inputLimits;
pidA.SetTunings(__kp, __ki, __kd);
pidB.SetTunings(__kp, __ki, __kd);
}
void rosCtrl(float rosX, float rosZ) {
setpointA = rosX - (rosZ * TRACK_WIDTH / 2.0);
setpointB = rosX + (rosZ * TRACK_WIDTH / 2.0);
setGoalSpeed(setpointA, setpointB);
}
void heartBeatCtrl() {
if (currentTimeMillis - lastCmdRecvTime > HEART_BEAT_DELAY) {
if (!heartbeatStopFlag) {
heartbeatStopFlag = true;
setGoalSpeed(0, 0);
// leftCtrl(0);
// rightCtrl(0);
}
}
}
void changeHeartBeatDelay(int inputCmd) {
HEART_BEAT_DELAY = inputCmd;
}
void mm_settings(byte inputMain, byte inputModule) {
mainType = inputMain;
moduleType = inputModule;
if (mainType == 1) {
WHEEL_D = 0.0800;
ONE_CIRCLE_PLUSES = 2100;
TRACK_WIDTH = 0.125;
SET_MOTOR_DIR = false; // checked
usePIDCompute = false;
} else if (mainType == 2) {
WHEEL_D = 0.0800;
ONE_CIRCLE_PLUSES = 1650;
TRACK_WIDTH = 0.172;
SET_MOTOR_DIR = true; // checked
usePIDCompute = false;
} else if (mainType == 3) {
WHEEL_D = 0.0523;
ONE_CIRCLE_PLUSES = 1092;
TRACK_WIDTH = 0.141;
SET_MOTOR_DIR = true; // checked
usePIDCompute = true;
}
plusesRate = 3.14159265359 * WHEEL_D / ONE_CIRCLE_PLUSES;
// initEncoders();
if (mainType == 1) {
screenLine_2 = "RaspRover";
} else if (mainType == 2) {
screenLine_2 = "UGV02";
} else if (mainType == 3) {
screenLine_2 = "UGV01";
}
if (moduleType == 0) {
screenLine_2 += " Null";
} else if (moduleType == 1) {
screenLine_2 += " Arm";
} else if (moduleType == 2) {
screenLine_2 += " PT";
}
}
oled_ctrl.h
+98
View File
@@ -0,0 +1,98 @@
// <<<<<<<<<<=== === ===SSD1306: 0x3C=== === ===>>>>>>>>>>
// 0.91inch OLED
bool screenDefaultMode = true;
unsigned long currentTimeMillis = millis();
unsigned long lastTimeMillis = millis();
// default
String screenLine_0;
String screenLine_1;
String screenLine_2;
String screenLine_3;
// custom
String customLine_0;
String customLine_1;
String customLine_2;
String customLine_3;
// #include <Adafruit_SSD1306.h>
#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 32 // OLED display height, in pixels
#define OLED_RESET -1 // Reset pin # (or -1 if sharing Arduino reset pin)
#define SCREEN_ADDRESS 0x3C ///< See datasheet for Address; 0x3D for 128x64, 0x3C for 128x32
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
// init oled ctrl functions.
void init_oled(){
if(!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
Serial.println(F("SSD1306 allocation failed"));
}
display.clearDisplay();
display.setTextSize(1);
display.setTextColor(SSD1306_WHITE);
display.setCursor(0,0);
display.display();
}
// Updata all data and flash the screen.
void oled_update() {
display.clearDisplay();
display.setCursor(0,0);
display.println(screenLine_0);
display.println(screenLine_1);
display.println(screenLine_2);
display.println(screenLine_3);
display.display();
}
// dev info update on oled.
void oledInfoUpdate() {
currentTimeMillis = millis();
if (currentTimeMillis - lastTimeMillis > 10000) {
inaDataUpdate();
lastTimeMillis = currentTimeMillis;
} else {
return;
}
if (!screenDefaultMode) {
return;
}
// inaDataUpdate();
screenLine_3 = "V:"+String(loadVoltage_V);
oled_update();
}
// oled ctrl.
void oledCtrl(byte inputLineNum, String inputMegs) {
screenDefaultMode = false;
switch (inputLineNum) {
case 0: customLine_0 = inputMegs;break;
case 1: customLine_1 = inputMegs;break;
case 2: customLine_2 = inputMegs;break;
case 3: customLine_3 = inputMegs;break;
}
display.clearDisplay();
display.setCursor(0,0);
display.println(customLine_0);
display.println(customLine_1);
display.println(customLine_2);
display.println(customLine_3);
display.display();
}
// set oled as default.
void setOledDefault(){
screenDefaultMode = true;
inaDataUpdate();
screenLine_3 = "V:"+String(loadVoltage_V);
oled_update();
lastTimeMillis = currentTimeMillis;
}
uart_ctrl.h
+516
View File
@@ -0,0 +1,516 @@
void jsonCmdReceiveHandler(){
int cmdType = jsonCmdReceive["T"].as<int>();
switch(cmdType){
case CMD_SPEED_CTRL: if (jsonCmdReceive.containsKey("T") &&
jsonCmdReceive.containsKey("L") &&
jsonCmdReceive.containsKey("R")){
if (jsonCmdReceive["L"].is<float>() &&
jsonCmdReceive["R"].is<float>()){
heartbeatStopFlag = false;
lastCmdRecvTime = millis();
setGoalSpeed(
jsonCmdReceive["L"],
jsonCmdReceive["R"]);
}
} break;
case CMD_PWM_INPUT: usePIDCompute = false;
heartbeatStopFlag = false;
lastCmdRecvTime = millis();
leftCtrl(jsonCmdReceive["L"]);
rightCtrl(jsonCmdReceive["R"]);
break;
case CMD_ROS_CTRL: rosCtrl(
jsonCmdReceive["X"],
jsonCmdReceive["Z"]);
heartbeatStopFlag = false;
lastCmdRecvTime = millis();break;
case CMD_SET_MOTOR_PID:
setPID(
jsonCmdReceive["P"],
jsonCmdReceive["I"],
jsonCmdReceive["D"],
jsonCmdReceive["L"]);break;
case CMD_OLED_CTRL: oledCtrl(
jsonCmdReceive["lineNum"],
jsonCmdReceive["Text"]);break;
case CMD_OLED_DEFAULT:setOledDefault();break;
case CMD_MODULE_TYPE: changeModuleType(
jsonCmdReceive["cmd"]);break;
case CMD_GET_IMU_DATA:
getIMUData();break;
case CMD_CALI_IMU_STEP:
imuCalibration();break;
case CMD_GET_IMU_OFFSET:
getIMUOffset();
break;
case CMD_SET_IMU_OFFSET:
setIMUOffset(
jsonCmdReceive["x"],
jsonCmdReceive["y"],
jsonCmdReceive["z"]);break;
case CMD_BASE_FEEDBACK:
baseInfoFeedback();break;
case CMD_BASE_FEEDBACK_FLOW:
setBaseInfoFeedbackMode(
jsonCmdReceive["cmd"]);break;
case CMD_FEEDBACK_FLOW_INTERVAL:
setFeedbackFlowInterval(
jsonCmdReceive["cmd"]);break;
case CMD_UART_ECHO_MODE:
setCmdEcho(
jsonCmdReceive["cmd"]);break;
case CMD_ARM_CTRL_UI: RoArmM2_uiCtrl(
jsonCmdReceive["E"],
jsonCmdReceive["Z"],
jsonCmdReceive["R"]
);break;
case CMD_LED_CTRL: led_pwm_ctrl(
jsonCmdReceive["IO4"],
jsonCmdReceive["IO5"]);break;
case CMD_GIMBAL_CTRL_SIMPLE:
gimbalCtrlSimple(
jsonCmdReceive["X"],
jsonCmdReceive["Y"],
jsonCmdReceive["SPD"],
jsonCmdReceive["ACC"]);break;
case CMD_GIMBAL_CTRL_MOVE:
gimbalCtrlMove(
jsonCmdReceive["X"],
jsonCmdReceive["Y"],
jsonCmdReceive["SX"],
jsonCmdReceive["SY"]);break;
case CMD_GIMBAL_CTRL_STOP:
gimbalCtrlStop();break;
case CMD_HEART_BEAT_SET:
changeHeartBeatDelay(
jsonCmdReceive["cmd"]);break;
case CMD_GIMBAL_STEADY:
gimbalSteadySet(
jsonCmdReceive["s"],
jsonCmdReceive["y"]);break;
case CMD_SET_SPD_RATE:
setSpdRate(
jsonCmdReceive["L"],
jsonCmdReceive["R"]);break;
case CMD_GET_SPD_RATE:
getSpdRate();break;
case CMD_SAVE_SPD_RATE:
saveSpdRate();break;
case CMD_GIMBAL_USER_CTRL:
gimbalUserCtrl(
jsonCmdReceive["X"],
jsonCmdReceive["Y"],
jsonCmdReceive["SPD"]);break;
// EoAT type settings.
case CMD_EOAT_TYPE: configEEmodeType(
jsonCmdReceive["mode"]);break;
case CMD_CONFIG_EOAT: configEoAT(
jsonCmdReceive["pos"],
jsonCmdReceive["ea"],
jsonCmdReceive["eb"]
);break;
// it moves to goal position directly
// with interpolation.
case CMD_MOVE_INIT: RoArmM2_moveInit();break;
case CMD_SINGLE_JOINT_CTRL:
RoArmM2_singleJointAbsCtrl(
jsonCmdReceive["joint"],
jsonCmdReceive["rad"],
jsonCmdReceive["spd"],
jsonCmdReceive["acc"]
);break;
case CMD_JOINTS_RAD_CTRL:
RoArmM2_allJointAbsCtrl(
jsonCmdReceive["base"],
jsonCmdReceive["shoulder"],
jsonCmdReceive["elbow"],
jsonCmdReceive["hand"],
jsonCmdReceive["spd"],
jsonCmdReceive["acc"]
);break;
case CMD_SINGLE_AXIS_CTRL:
RoArmM2_singlePosAbsBesselCtrl(
jsonCmdReceive["axis"],
jsonCmdReceive["pos"],
jsonCmdReceive["spd"]
);break;
case CMD_XYZT_GOAL_CTRL:
RoArmM2_allPosAbsBesselCtrl(
jsonCmdReceive["x"],
jsonCmdReceive["y"],
jsonCmdReceive["z"],
jsonCmdReceive["t"],
jsonCmdReceive["spd"]
);break;
case CMD_XYZT_DIRECT_CTRL:
RoArmM2_baseCoordinateCtrl(
jsonCmdReceive["x"],
jsonCmdReceive["y"],
jsonCmdReceive["z"],
jsonCmdReceive["t"]);
RoArmM2_goalPosMove();
break;
case CMD_SERVO_RAD_FEEDBACK:
RoArmM2_getPosByServoFeedback();
RoArmM2_infoFeedback();
break;
case CMD_EOAT_HAND_CTRL:
RoArmM2_handJointCtrlRad(1,
jsonCmdReceive["cmd"],
jsonCmdReceive["spd"],
jsonCmdReceive["acc"]
);break;
case CMD_EOAT_GRAB_TORQUE:
RoArmM2_handTorqueCtrl(
jsonCmdReceive["tor"]
);break;
case CMD_SET_JOINT_PID:
RoArmM2_setJointPID(
jsonCmdReceive["joint"],
jsonCmdReceive["p"],
jsonCmdReceive["i"]
);break;
case CMD_RESET_PID: RoArmM2_resetPID();break;
// set a new x-axis.
case CMD_SET_NEW_X: setNewAxisX(
jsonCmdReceive["xAxisAngle"]
);break;
case CMD_DELAY_MILLIS:
RoArmM2_delayMillis(
jsonCmdReceive["cmd"]
);break;
case CMD_DYNAMIC_ADAPTATION:
RoArmM2_dynamicAdaptation(
jsonCmdReceive["mode"],
jsonCmdReceive["b"],
jsonCmdReceive["s"],
jsonCmdReceive["e"],
jsonCmdReceive["h"]
);break;
// this two funcs are NOT for UGV.
// case CMD_SWITCH_CTRL: switchCtrl(
// jsonCmdReceive["pwm_a"],
// jsonCmdReceive["pwm_b"]
// );break;
// case CMD_LIGHT_CTRL: lightCtrl(
// jsonCmdReceive["led"]
// );break;
case CMD_SWITCH_OFF: switchEmergencyStop();break;
case CMD_SINGLE_JOINT_ANGLE:
RoArmM2_singleJointAngleCtrl(
jsonCmdReceive["joint"],
jsonCmdReceive["angle"],
jsonCmdReceive["spd"],
jsonCmdReceive["acc"]
);break;
case CMD_JOINTS_ANGLE_CTRL:
RoArmM2_allJointsAngleCtrl(
jsonCmdReceive["b"],
jsonCmdReceive["s"],
jsonCmdReceive["e"],
jsonCmdReceive["h"],
jsonCmdReceive["spd"],
jsonCmdReceive["acc"]
);break;
// constant ctrl
// m: 0 - angle
// 1 - xyzt
// cmd: 0 - stop
// 1 - increase
// 2 - decrease
// {"T":123,"m":0,"axis":0,"cmd":0,"spd":0}
case CMD_CONSTANT_CTRL:
constantCtrl(
jsonCmdReceive["m"],
jsonCmdReceive["axis"],
jsonCmdReceive["cmd"],
jsonCmdReceive["spd"]
);break;
// mission & steps edit & file edit.
case CMD_SCAN_FILES: scanFlashContents();
break;
case CMD_CREATE_FILE: createFile(
jsonCmdReceive["name"],
jsonCmdReceive["content"]
);break;
case CMD_READ_FILE: readFile(
jsonCmdReceive["name"]
);break;
case CMD_DELETE_FILE: deleteFile(
jsonCmdReceive["name"]
);break;
case CMD_APPEND_LINE: appendLine(
jsonCmdReceive["name"],
jsonCmdReceive["content"]
);break;
case CMD_INSERT_LINE: insertLine(
jsonCmdReceive["name"],
jsonCmdReceive["lineNum"],
jsonCmdReceive["content"]
);break;
case CMD_REPLACE_LINE:
replaceLine(
jsonCmdReceive["name"],
jsonCmdReceive["lineNum"],
jsonCmdReceive["content"]
);break;
case CMD_READ_LINE: readSingleLine(
jsonCmdReceive["name"],
jsonCmdReceive["lineNum"]
);break;
case CMD_DELETE_LINE: deleteSingleLine(
jsonCmdReceive["name"],
jsonCmdReceive["lineNum"]
);break;
case CMD_TORQUE_CTRL: servoTorqueCtrl(254,
jsonCmdReceive["cmd"]);
break;
case CMD_CREATE_MISSION:
createMission(
jsonCmdReceive["name"],
jsonCmdReceive["intro"]
);break;
case CMD_MISSION_CONTENT:
missionContent(
jsonCmdReceive["name"]
);break;
case CMD_APPEND_STEP_JSON:
appendStepJson(
jsonCmdReceive["name"],
jsonCmdReceive["step"]
);break;
case CMD_APPEND_STEP_FB:
appendStepFB(
jsonCmdReceive["name"],
jsonCmdReceive["spd"]
);break;
case CMD_APPEND_DELAY:
appendDelayCmd(
jsonCmdReceive["name"],
jsonCmdReceive["delay"]
);break;
case CMD_INSERT_STEP_JSON:
insertStepJson(
jsonCmdReceive["name"],
jsonCmdReceive["stepNum"],
jsonCmdReceive["step"]
);break;
case CMD_INSERT_STEP_FB:
insertStepFB(
jsonCmdReceive["name"],
jsonCmdReceive["stepNum"],
jsonCmdReceive["spd"]
);break;
case CMD_INSERT_DELAY:
insertDelayCmd(
jsonCmdReceive["name"],
jsonCmdReceive["stepNum"],
jsonCmdReceive["spd"]
);break;
case CMD_REPLACE_STEP_JSON:
replaceStepJson(
jsonCmdReceive["name"],
jsonCmdReceive["stepNum"],
jsonCmdReceive["step"]
);break;
case CMD_REPLACE_STEP_FB:
replaceStepFB(
jsonCmdReceive["name"],
jsonCmdReceive["stepNum"],
jsonCmdReceive["spd"]
);break;
case CMD_REPLACE_DELAY:
replaceDelayCmd(
jsonCmdReceive["name"],
jsonCmdReceive["stepNum"],
jsonCmdReceive["delay"]
);break;
case CMD_DELETE_STEP: deleteStep(
jsonCmdReceive["name"],
jsonCmdReceive["stepNum"]
);break;
case CMD_MOVE_TO_STEP:
moveToStep(
jsonCmdReceive["name"],
jsonCmdReceive["stepNum"]
);break;
case CMD_MISSION_PLAY:
missionPlay(
jsonCmdReceive["name"],
jsonCmdReceive["times"]
);break;
// esp-now settings.
case CMD_BROADCAST_FOLLOWER:
changeBroadcastMode(
jsonCmdReceive["mode"],
jsonCmdReceive["mac"]
);break;
case CMD_ESP_NOW_CONFIG:
changeEspNowMode(
jsonCmdReceive["mode"]
);break;
case CMD_GET_MAC_ADDRESS:
getThisDevMacAddress();
break;
case CMD_ESP_NOW_ADD_FOLLOWER:
registerNewFollowerToPeer(
jsonCmdReceive["mac"]);break;
case CMD_ESP_NOW_REMOVE_FOLLOWER:
deleteFollower(
jsonCmdReceive["mac"]);break;
case CMD_ESP_NOW_GROUP_CTRL:
espNowGroupSend(
jsonCmdReceive["dev"],
jsonCmdReceive["b"],
jsonCmdReceive["s"],
jsonCmdReceive["e"],
jsonCmdReceive["h"],
jsonCmdReceive["cmd"],
jsonCmdReceive["megs"]
);break;
case CMD_ESP_NOW_SINGLE:
espNowSingleDevSend(
jsonCmdReceive["mac"],
jsonCmdReceive["dev"],
jsonCmdReceive["b"],
jsonCmdReceive["s"],
jsonCmdReceive["e"],
jsonCmdReceive["h"],
jsonCmdReceive["cmd"],
jsonCmdReceive["megs"]
);break;
// wifi settings.
case CMD_WIFI_ON_BOOT:
configWifiModeOnBoot(
jsonCmdReceive["cmd"]
);break;
case CMD_SET_AP: wifiModeAP(
jsonCmdReceive["ssid"],
jsonCmdReceive["password"]
);break;
case CMD_SET_STA: wifiModeSTA(
jsonCmdReceive["ssid"],
jsonCmdReceive["password"]
);break;
case CMD_WIFI_APSTA: wifiModeAPSTA(
jsonCmdReceive["ap_ssid"],
jsonCmdReceive["ap_password"],
jsonCmdReceive["sta_ssid"],
jsonCmdReceive["sta_password"]
);break;
case CMD_WIFI_INFO: wifiStatusFeedback();break;
case CMD_WIFI_CONFIG_CREATE_BY_STATUS:
createWifiConfigFileByStatus();break;
case CMD_WIFI_CONFIG_CREATE_BY_INPUT:
createWifiConfigFileByInput(
jsonCmdReceive["mode"],
jsonCmdReceive["ap_ssid"],
jsonCmdReceive["ap_password"],
jsonCmdReceive["sta_ssid"],
jsonCmdReceive["sta_password"]
);break;
case CMD_WIFI_STOP: wifiStop();break;
// servo settings.
case CMD_SET_SERVO_ID:
changeID(
jsonCmdReceive["raw"],
jsonCmdReceive["new"]
);break;
case CMD_SET_MIDDLE: setMiddlePos(
jsonCmdReceive["id"]
);break;
case CMD_SET_SERVO_PID:
setServosPID(
jsonCmdReceive["id"],
jsonCmdReceive["p"]
);break;
// esp-32 dev ctrl.
case CMD_REBOOT: esp_restart();break;
case CMD_FREE_FLASH_SPACE:
freeFlashSpace();break;
case CMD_BOOT_MISSION_INFO:
missionContent("boot");break;
case CMD_RESET_BOOT_MISSION:
deleteFile("boot.mission");
createFile("boot", "these cmds run automatically at boot.");
break;
case CMD_NVS_CLEAR: nvs_flash_erase();
delay(1000);
nvs_flash_init();
break;
case CMD_INFO_PRINT: configInfoPrint(
jsonCmdReceive["cmd"]
);break;
// case CMD_PID_RESET_A: PID_v2 pidA(__kp, __ki, __kd, PID::Direct);
// PID_v2 pidB(__kp, __ki, __kd, PID::Direct);
// pidControllerInit();break;
// mainType & moduleType settings.
case CMD_MM_TYPE_SET: mm_settings(
jsonCmdReceive["main"],
jsonCmdReceive["module"]
);
break;
}
}
void serialCtrl() {
static String receivedData;
while (Serial.available() > 0) {
char receivedChar = Serial.read();
receivedData += receivedChar;
// Detect the end of the JSON string based on a specific termination character
if (receivedChar == '\n') {
// Now we have received the complete JSON string
DeserializationError err = deserializeJson(jsonCmdReceive, receivedData);
if (err == DeserializationError::Ok) {
if (InfoPrint == 1 && uartCmdEcho) {
Serial.print(receivedData);
}
jsonCmdReceiveHandler();
} else {
// Handle JSON parsing error here
}
// Reset the receivedData for the next JSON string
receivedData = "";
}
}
}
ugv_advance.h
+452
View File
@@ -0,0 +1,452 @@
// advance funcs for RoArm-M2 ctrl
// place holder.
void jsonCmdReceiveHandler();
bool moveToStep(String inputName, int inputStepNum);
// mission abort after serial received anything.
bool serialMissionAbort() {
if (Serial.available()) {
if (InfoPrint == 1) {Serial.println("[missionPlay abort.]");}
return true;
} else {
return false;
}
}
// input the mission name and the intro to create a mission file.
bool createMission(String inputName, String inputIntro) {
jsonInfoSend.clear();
jsonInfoSend["name"] = inputName;
jsonInfoSend["intro"] = inputIntro;
String contentBuffer;
serializeJson(jsonInfoSend, contentBuffer);
jsonInfoSend.clear();
return createFile(inputName + ".mission", contentBuffer);
}
// input the mission name and get the total content
int missionContent(String inputName) {
File file = LittleFS.open("/" + inputName + ".mission", "r");
if (!file) {
Serial.println("file not found.");
return -1;
}
Serial.println("---=== File Content ===---");
Serial.println("reading file: [" + inputName + "] starts:\n");
String mission_intro = file.readStringUntil('\n');
Serial.println(mission_intro);
jsonInfoHttp.clear();
jsonInfoHttp["info"] = "reading mission.";
jsonInfoHttp["first_line"] = mission_intro;
int _LineNum = 0;
while (file.available()) {
_LineNum++;
String line = file.readStringUntil('\n');
Serial.print("[StepNum: ");Serial.print(_LineNum);Serial.print(" ] - ");
Serial.println(line);
jsonInfoHttp["StepNum_"+String(_LineNum)] = line;
}
Serial.println("^^^ ^^^ ^^^ reading file: " + inputName + ".mission ends. ^^^ ^^^ ^^^");
file.close();
return _LineNum;
}
// input the mission name and the step to append
// a new step at the end of the mission.
// using inputStep(String)
bool appendStepJson(String inputName, String inputStep) {
DeserializationError err = deserializeJson(jsonInfoSend, inputStep);
if (err == DeserializationError::Ok) {
if (InfoPrint == 1) {
Serial.println("[json parsing succeed.]");
}
appendLine(inputName + ".mission", inputStep);
jsonInfoSend.clear();
return true;
} else {
jsonInfoSend.clear();
if (InfoPrint == 1) {
Serial.println("[deserializeJson err]");
}
return false;
}
}
// input the mission name and the step to append
// a new step at the end of the mission.
// using feedback.
void appendStepFB(String inputName, float inputSpd) {
RoArmM2_infoFeedback();
jsonInfoSend.clear();
jsonInfoSend["T"] = 104;
jsonInfoSend["x"] = lastX;
jsonInfoSend["y"] = lastY;
jsonInfoSend["z"] = lastZ;
jsonInfoSend["t"] = lastT;
jsonInfoSend["spd"] = inputSpd;
String contentBuffer;
serializeJson(jsonInfoSend, contentBuffer);
appendLine(inputName + ".mission", contentBuffer);
}
// append a new delay(ms) at the end of the mission.
void appendDelayCmd(String inputName, int delayTime) {
jsonInfoSend.clear();
jsonInfoSend["T"] = 111;
jsonInfoSend["cmd"] = delayTime;
String contentBuffer;
serializeJson(jsonInfoSend, contentBuffer);
appendLine(inputName + ".mission", contentBuffer);
}
// insert a new step as the stepNum
// using the json string input.
bool insertStepJson(String inputName, int inputStepNum, String inputStep) {
DeserializationError err = deserializeJson(jsonInfoSend, inputStep);
if (err == DeserializationError::Ok) {
if (InfoPrint == 1) {
Serial.println("[json parsing succeed.]");
}
insertLine(inputName + ".mission", inputStepNum + 1, inputStep);
jsonInfoSend.clear();
return true;
} else {
jsonInfoSend.clear();
if (InfoPrint == 1) {
Serial.println("[deserializeJson err]");
}
return false;
}
}
// insert a new step as the stepNum
// using the feedback.
void insertStepFB(String inputName, int inputStepNum, float inputSpd) {
RoArmM2_infoFeedback();
jsonInfoSend.clear();
jsonInfoSend["T"] = 104;
jsonInfoSend["x"] = lastX;
jsonInfoSend["y"] = lastY;
jsonInfoSend["z"] = lastZ;
jsonInfoSend["t"] = lastT;
jsonInfoSend["spd"] = inputSpd;
String contentBuffer;
serializeJson(jsonInfoSend, contentBuffer);
insertLine(inputName + ".mission", inputStepNum + 1, contentBuffer);
}
// insert a new delayCmd as the stepNum
void insertDelayCmd(String inputName, int inputStepNum, int delayTime) {
jsonInfoSend.clear();
jsonInfoSend["T"] = 111;
jsonInfoSend["cmd"] = delayTime;
String contentBuffer;
serializeJson(jsonInfoSend, contentBuffer);
insertLine(inputName + ".mission", inputStepNum + 1, contentBuffer);
}
// replace the cmd at stepNum.
// using the step json string input.
bool replaceStepJson(String inputName, int inputStepNum, String inputStep) {
DeserializationError err = deserializeJson(jsonInfoSend, inputStep);
if (err == DeserializationError::Ok) {
if (InfoPrint == 1) {
Serial.println("[json parsing succeed.]");
}
replaceLine(inputName + ".mission", inputStepNum + 1, inputStep);
jsonInfoSend.clear();
return true;
} else {
jsonInfoSend.clear();
if (InfoPrint == 1) {
Serial.println("[deserializeJson err]");
}
return false;
}
}
// replace the cmd at stepNum.
// using feedback.
void replaceStepFB(String inputName, int inputStepNum, float inputSpd) {
RoArmM2_infoFeedback();
jsonInfoSend.clear();
jsonInfoSend["T"] = 104;
jsonInfoSend["x"] = lastX;
jsonInfoSend["y"] = lastY;
jsonInfoSend["z"] = lastZ;
jsonInfoSend["t"] = lastT;
jsonInfoSend["spd"] = inputSpd;
String contentBuffer;
serializeJson(jsonInfoSend, contentBuffer);
replaceLine(inputName + ".mission", inputStepNum + 1, contentBuffer);
}
// replace the cmd at stepNum with delay cmd.
void replaceDelayCmd(String inputName, int inputStepNum, int delayTime) {
jsonInfoSend.clear();
jsonInfoSend["T"] = 111;
jsonInfoSend["cmd"] = delayTime;
String contentBuffer;
serializeJson(jsonInfoSend, contentBuffer);
replaceLine(inputName + ".mission", inputStepNum + 1, contentBuffer);
}
// delete a step
void deleteStep(String inputName, int inputStepNum) {
deleteSingleLine(inputName + ".mission", inputStepNum + 1);
}
// input the mission name and the stepNum.
// it will process the cmd.
bool moveToStep(String inputName, int inputStepNum) {
String stepStringBuffer = readSingleLine(inputName + ".mission", inputStepNum + 1);
DeserializationError err = deserializeJson(jsonCmdReceive, stepStringBuffer);
if (err == DeserializationError::Ok) {
if (InfoPrint == 1) {
Serial.println("[json parsing succeed.]");
Serial.println("[import a step]");
Serial.print("[mission name]: ");Serial.println(inputName);
Serial.print("[stepNum]: ");Serial.println(inputStepNum);
Serial.print("[cmd]: ");Serial.println(stepStringBuffer);
}
jsonCmdReceiveHandler();
if (InfoPrint == 1) {
Serial.println("[step finished]");
}
jsonInfoSend.clear();
return true;
} else {
jsonInfoSend.clear();
if (InfoPrint == 1) {
Serial.println("[deserializeJson err]");
}
return false;
}
}
// input the mission name and the repeat times.
// when repeatTimes = -1, it will loop forever.
// play a mission file.
void missionPlay(String inputName, int repeatTimes) {
int _LineNum = missionContent(inputName);
int currentTimes = 0;
while (1) {
currentTimes++;
if (currentTimes > repeatTimes && repeatTimes != -1) {
if (InfoPrint == 1) {Serial.println("[missionPlay finished.]");}
return;
}
if (InfoPrint == 1) {
Serial.print("---\n[currentTimes: ");Serial.print(currentTimes);
Serial.println(" ]");
}
for (int i = 1; i<=_LineNum; i++) {
if (serialMissionAbort()) {
return;
}
moveToStep(inputName, i);
}
}
}
// change EEmode.
void configEEmodeType(byte inputMode) {
EEMode = inputMode;
if (inputMode == 0){
l3A = ARM_L3_LENGTH_MM_A_0;
l3B = ARM_L3_LENGTH_MM_B_0;
l3 = sqrt(l3A * l3A + l3B * l3B);
t3rad = atan2(l3B, l3A);
initX = l3A + l2B;
initY = 0;
initZ = l2A - l3B;
initT = M_PI;
}
else if (inputMode == 1){
l3A = ARM_L3_LENGTH_MM_A_1;
l3B = ARM_L3_LENGTH_MM_B_1;
l3 = sqrt(l3A * l3A + l3B * l3B);
t3rad = atan2(l3B, l3A);
EoAT_A = EoAT_A;
EoAT_B = EoAT_B;
l4A = ARM_L4_LENGTH_MM_A;
l4B = ARM_L4_LENGTH_MM_B;
lEA = EoAT_A + ARM_L4_LENGTH_MM_A;
lEB = EoAT_B + ARM_L4_LENGTH_MM_B;
lE = sqrt(lEA * lEA + lEB * lEB);
tErad = atan2(lEB, lEA);
initX = l3A + l2B + l4A + EoAT_A;
initY = 0;
initZ = l2A - l3B - l4B - EoAT_B;
initT = M_PI;
}
goalX = initX;
goalY = initY;
goalZ = initZ;
goalT = initT;
lastX = goalX;
lastY = goalY;
lastZ = goalZ;
lastT = goalT;
RoArmM2_baseCoordinateCtrl(initX, initY, initZ, initT);
RoArmM2_goalPosMove();
}
// config the siza of EoAT.
void configEoAT(byte mountPos, double inputEA, double inputEB) {
switch (mountPos) {
case 0: ARM_L4_LENGTH_MM_A = 67.85;break;
case 1: ARM_L4_LENGTH_MM_A = 64.16;break;
case 2: ARM_L4_LENGTH_MM_A = 59.07;break;
case 3: ARM_L4_LENGTH_MM_A = 51.07;break;
}
EoAT_A = inputEA;
EoAT_B = inputEB;
l4A = ARM_L4_LENGTH_MM_A;
l4B = ARM_L4_LENGTH_MM_B;
lEA = EoAT_A + ARM_L4_LENGTH_MM_A;
lEB = EoAT_B + ARM_L4_LENGTH_MM_B;
lE = sqrt(lEA * lEA + lEB * lEB);
tErad = atan2(lEB, lEA);
initX = l3A + l2B + l4A + EoAT_A;
initY = 0;
initZ = l2A - l3B - l4B - EoAT_B;
initT = M_PI;
}
// set the InfoPrint.
void configInfoPrint(byte inputCmd) {
switch (inputCmd) {
case 0: InfoPrint = 0;
break;
case 1: InfoPrint = 1;
break;
case 2: InfoPrint = 2;
break;
}
}
// set the baseInfoFeedback.
void setBaseInfoFeedbackMode(bool inputCmd) {
if (inputCmd == 1) {
baseFeedbackFlow = 1;
} else if (inputCmd == 0) {
baseFeedbackFlow = 0;
}
}
// baseInfoFeedback.
void baseInfoFeedback() {
static unsigned long last_feedback_time;
if (millis() - last_feedback_time < feedbackFlowExtraDelay) {
return;
}
last_feedback_time = millis();
jsonInfoHttp.clear();
jsonInfoHttp["T"] = FEEDBACK_BASE_INFO;
jsonInfoHttp["L"] = speedGetA;
jsonInfoHttp["R"] = speedGetB;
jsonInfoHttp["r"] = icm_roll;
jsonInfoHttp["p"] = icm_pitch;
jsonInfoHttp["y"] = icm_yaw;
// jsonInfoHttp["q0"] = qw;
// jsonInfoHttp["q1"] = qx;
// jsonInfoHttp["q2"] = qy;
// jsonInfoHttp["q3"] = qz;
jsonInfoHttp["temp"] = icm_temp;
jsonInfoHttp["v"] = loadVoltage_V;
switch(moduleType) {
case 1:
jsonInfoHttp["x"] = lastX;
jsonInfoHttp["y"] = lastY;
jsonInfoHttp["z"] = lastZ;
jsonInfoHttp["b"] = radB;
jsonInfoHttp["s"] = radS;
jsonInfoHttp["e"] = radE;
jsonInfoHttp["t"] = lastT;
jsonInfoHttp["torB"] = servoFeedback[BASE_SERVO_ID - 11].load;
jsonInfoHttp["torS"] = servoFeedback[SHOULDER_DRIVING_SERVO_ID - 11].load - servoFeedback[SHOULDER_DRIVEN_SERVO_ID - 11].load;
jsonInfoHttp["torE"] = servoFeedback[ELBOW_SERVO_ID - 11].load;
jsonInfoHttp["torH"] = servoFeedback[GRIPPER_SERVO_ID - 11].load;
break;
case 2:
jsonInfoHttp["pan"] = panAngleCompute(gimbalFeedback[0].pos);
jsonInfoHttp["tilt"] = tiltAngleCompute(gimbalFeedback[1].pos);
break;
}
String getInfoJsonString;
serializeJson(jsonInfoHttp, getInfoJsonString);
Serial.println(getInfoJsonString);
}
// change module type.
void changeModuleType(byte inputCmd) {
moduleType = inputCmd;
}
void setFeedbackFlowInterval(int inputCmd) {
feedbackFlowExtraDelay = abs(inputCmd);
}
void setCmdEcho(bool inputCmd) {
uartCmdEcho = inputCmd;
}
void saveSpdRate() {
jsonInfoHttp.clear();
jsonInfoHttp["T"] = CMD_SET_SPD_RATE;
jsonInfoHttp["L"] = spd_rate_A;
jsonInfoHttp["R"] = spd_rate_B;
String getInfoJsonString;
serializeJson(jsonInfoHttp, getInfoJsonString);
appendStepJson("boot", getInfoJsonString);
}
ugv_config.h
+378
View File
@@ -0,0 +1,378 @@
// the uart used to control servos.
// GPIO 18 - S_RXD, GPIO 19 - S_TXD, as default.
#define RoArmM2_Servo_RXD 18
#define RoArmM2_Servo_TXD 19
// 2: flow feedback.
// 1: [default]print debug info in serial.
// 0: don't print debug info in serial.
byte InfoPrint = 1;
// devices info:
// espNowMode: 0 - none
// 1 - flow-leader(group): sending cmds
// 2 - flow-leader(single): sending cmds to a single follower
// 3 - [default]follower: recv cmds
byte espNowMode = 3;
// set the broadcast ctrl mode.
// broadcast mac address: FF:FF:FF:FF:FF:FF.
// true - [default]it can be controled by broadcast mac address.
// false - it won't be controled by broadcast mac address.
bool ctrlByBroadcast = true;
// you can define some whitelist mac addresses here.
uint8_t mac_whitelist_broadcast[] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
// Multifunction End-Effector Switching System.
// 0 - end servo as grab.
// 1 - end servo as a joint moving in vertical plane.
byte EEMode = 0;
// run new json cmd
bool runNewJsonCmd = false;
// 1: WAVE ROVER
// 2: UGV02(UGV)
// 3: UGV01(UGV)
byte mainType = 1;
// 0: [Base default] without RoArm-M2 and gimbal.
// 1: [RoArm default] RoArm-M2 mounted on the UGV.
// 2: [Gimbal default] Gimbal mounted on the UGV.
byte moduleType = 0;
// false: gimbal steady mode off.
// true: gimbal steady mode on.
bool steadyMode = false;
// 0: turn off base info feedback flow.
// 1: [default] turn on base info feedback flow.
bool baseFeedbackFlow = 0;
String thisMacStr;
#define BASE_JOINT 1
#define SHOULDER_JOINT 2
#define ELBOW_JOINT 3
#define EOAT_JOINT 4
// define servoID
// |---[14]---|
// || | | ||
// || ||
// || | | ||
// || ||
// || | | ||
// || -[15]- ||
// || ||
// ||[13][12]||
// | __ |
// [11]
#define BASE_SERVO_ID 11
#define SHOULDER_DRIVING_SERVO_ID 12
#define SHOULDER_DRIVEN_SERVO_ID 13
#define ELBOW_SERVO_ID 14
#define GRIPPER_SERVO_ID 15
#define ARM_SERVO_MIDDLE_POS 2047
#define ARM_SERVO_MIDDLE_ANGLE 180
#define ARM_SERVO_POS_RANGE 4096
#define ARM_SERVO_ANGLE_RANGE 360
#define ARM_SERVO_INIT_SPEED 600
#define ARM_SERVO_INIT_ACC 20
#define ARM_L1_LENGTH_MM 126.06
#define ARM_L2_LENGTH_MM_A 236.82
#define ARM_L2_LENGTH_MM_B 30.00
#define ARM_L3_LENGTH_MM_A_0 280.15
#define ARM_L3_LENGTH_MM_B_0 1.73
// TYPE:0
// -------L3A-----------O==L2B===
// | ^ ||
// L3B | ||
// | ELBOW_JOINT ||
// L2A
// ||
// ||
// ||
// SHOULDER_JOINT -> OO
// [||]
// L1
// [||]
// BASE_JOINT -> X
double l1 = ARM_L1_LENGTH_MM;
double l2A = ARM_L2_LENGTH_MM_A;
double l2B = ARM_L2_LENGTH_MM_B;
double l2 = sqrt(l2A * l2A + l2B * l2B);
double t2rad = atan2(l2B, l2A);
double l3A = ARM_L3_LENGTH_MM_A_0;
double l3B = ARM_L3_LENGTH_MM_B_0;
double l3 = sqrt(l3A * l3A + l3B * l3B);
double t3rad = atan2(l3B, l3A);
#define ARM_L3_LENGTH_MM_A_1 215.99
#define ARM_L3_LENGTH_MM_B_1 0
// edge
double ARM_L4_LENGTH_MM_A = 67.85;
// D-3.2
// double ARM_L4_LENGTH_MM_A = 64.16;
// D-4.2
// double ARM_L4_LENGTH_MM_A = 59.07;
// D-10.2
// double ARM_L4_LENGTH_MM_A = 51.07;
#define ARM_L4_LENGTH_MM_B 5.98
// TYPE:1
// -------L3A-----------O==L2B===
// | ^ ||
// L3B | ||
// | ELBOW_JOINT ||
// ---L4A---O L2A
// | ||
// | L4B ||
// / | ||
// 180°X-EA-X SHOULDER_JOINT -> OO
// \ | [||]
// EB L1
// | [||]
// -------- BASE_JOINT -> XX
// \ T:210°
// \
// EB
// \
// -----------
double EoAT_A = 0;
double EoAT_B = 0;
double l4A = ARM_L4_LENGTH_MM_A;
double l4B = ARM_L4_LENGTH_MM_B;
double lEA = EoAT_A + ARM_L4_LENGTH_MM_A;
double lEB = EoAT_B + ARM_L4_LENGTH_MM_B;
double lE = sqrt(lEA * lEA + lEB * lEB);
double tErad = atan2(lEB, lEA);
double initX = l3A+l2B; //
double initY = 0;
double initZ = l2A-l3B;
double initT = M_PI;
double goalX = initX;
double goalY = initY;
double goalZ = initZ;
double goalT = initT;
double lastX = goalX;
double lastY = goalY;
double lastZ = goalZ;
double lastT = goalT;
double base_r;
double delta_x;
double delta_y;
double beta_x;
double beta_y;
double radB;
double radS;
double radE;
double radG;
#define MAX_SERVO_ID 32 // MAX:253
// the uart used to control servos.
// GPIO 18 - S_RXD, GPIO 19 - S_TXD, as default.
#define S_RXD 18
#define S_TXD 19
double BASE_JOINT_RAD = 0;
double SHOULDER_JOINT_RAD = 0;
double ELBOW_JOINT_RAD = M_PI/2;
double EOAT_JOINT_RAD = M_PI;
double EOAT_JOINT_RAD_BUFFER;
double BASE_JOINT_ANG = 0;
double SHOULDER_JOINT_ANG = 0;
double ELBOW_JOINT_ANG = 90.0;
double EOAT_JOINT_ANG = 180.0;
// true: torqueLock ON, servo produces torque.
// false: torqueLock OFF, servo release torque.
bool RoArmM2_torqueLock = true;
bool emergencyStopFlag = false;
bool newCmdReceived = false;
bool nanIK;
bool RoArmM2_initCheckSucceed = false;
// bool RoArmM2_initCheckSucceed = true;
// // // args for syncWritePos.
u8 servoID[5] = {11, 12, 13, 14, 15};
s16 goalPos[5] = {2047, 2047, 2047, 2047, 2047};
u16 moveSpd[5] = {0, 0, 0, 0, 0};
u8 moveAcc[5] = {ARM_SERVO_INIT_ACC,
ARM_SERVO_INIT_ACC,
ARM_SERVO_INIT_ACC,
ARM_SERVO_INIT_ACC,
ARM_SERVO_INIT_ACC};
double ARM_BASE_LIMIT_MIN_RAD = -M_PI/2;
double ARM_BASE_LIMIT_MAX_RAD = M_PI/2;
double ARM_SHOULDER_LIMIT_MIN_RAD = -M_PI/2;
double ARM_SHOULDER_LIMIT_MAX_RAD = M_PI/2;
double ARM_ELBOW_LIMIT_MIN_RAD = -M_PI/2;
double ARM_ELBOW_LIMIT_MAX_RAD = M_PI/2;
double ARM_GRIPPER_LIMIT_MIN_RAD = -M_PI/2;
double ARM_GRIPPER_LIMIT_MAX_RAD = M_PI/2;
// --- --- --- Pneumatic Components && Lights --- --- ---
const uint16_t ANALOG_WRITE_BITS = 8;
const uint16_t MAX_PWM = pow(2, ANALOG_WRITE_BITS)-1;
const uint16_t MIN_PWM = MAX_PWM/4;
#define PWMA 25 // Motor A PWM control
#define AIN2 17 // Motor A input 2
#define AIN1 21 // Motor A input 1
#define BIN1 22 // Motor B input 1
#define BIN2 23 // Motor B input 2
#define PWMB 26 // Motor B PWM control
#define AENCA 35 // Encoder A input
#define AENCB 34
#define BENCB 16 // Encoder B input
#define BENCA 27
int freq = 100000;
int channel_A = 5;
int channel_B = 6;
// --- --- --- Bus Servo Settings --- --- ---
#define ST_PID_P_ADDR 21
#define ST_PID_D_ADDR 22
#define ST_PID_I_ADDR 23
#define ST_PID_ROARM_P 16
#define ST_PID_DEFAULT_P 32
#define ST_TORQUE_MAX 1000
#define ST_TORQUE_MIN 50
// --- --- --- i2c Settings --- --- ---
#define S_SCL 33
#define S_SDA 32
// --- --- --- web / constant moving --- --- ---
#define MOVE_STOP 0
#define MOVE_INCREASE 1
#define MOVE_DECREASE 2
#define CONST_ANGLE 0
#define CONST_XYZT 1
float const_spd;
byte const_mode;
byte const_cmd_base_x;
byte const_cmd_shoulder_y;
byte const_cmd_elbow_z;
byte const_cmd_eoat_t;
float const_goal_base = BASE_JOINT_ANG;
float const_goal_shoulder = SHOULDER_JOINT_ANG;
float const_goal_elbow = ELBOW_JOINT_ANG;
float const_goal_eoat = EOAT_JOINT_ANG;
unsigned long prev_time = 0;
String jsonFeedbackWeb = "";
// --- --- --- pid controller --- --- ---
float __kp = 20.0;
float __ki = 2000.0;
float __kd = 0;
float windup_limits = 255;
// --- --- --- ugv base --- --- ---
#define THRESHOLD_PWM 23
// mainType:01 RaspRover
// #define WHEEL_D 0.0800
// #define ONE_CIRCLE_PLUSES 2100
// #define TRACK_WIDTH 0.125
// #define SET_MOTOR_DIR false
// mainType:02 UGV Rover
// #define WHEEL_D 0.0800
// #define ONE_CIRCLE_PLUSES 1650
// #define TRACK_WIDTH 0.172
// #define SET_MOTOR_DIR false
// mainType:03 UGV Beast
// #define WHEEL_D 0.0523
// #define ONE_CIRCLE_PLUSES 1092
// #define TRACK_WIDTH 0.141
// #define SET_MOTOR_DIR true
double WHEEL_D = 0.0800;
int ONE_CIRCLE_PLUSES = 1650;
double TRACK_WIDTH = 0.172;
bool SET_MOTOR_DIR = false;
#define IO4_PIN 4
#define IO5_PIN 5
int IO4_CH = 7;
int IO5_CH = 8;
const uint16_t FREQ = 200;
int feedbackFlowExtraDelay = 0;
bool uartCmdEcho = 1;
#define GIMBAL_PAN_ID 2
#define GIMBAL_TILT_ID 1
#define SERVO_STOP_DELAY 3
int HEART_BEAT_DELAY = 3000;
unsigned long lastCmdRecvTime = millis();
// --- --- --- ugv imu --- --- ---
double icm_pitch, icm_roll, icm_yaw, icm_temp;
unsigned long last_imu_update = 0;
double qw, qx, qy, qz;
double ax, ay, az;
double mx, my, mz;
double gx, gy, gz;
ugv_led_ctrl.h
+15
View File
@@ -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));
}
web_page.h
+1034
View File
File diff suppressed because it is too large Load Diff
wifi_ctrl.h
+398
View File
@@ -0,0 +1,398 @@
// wifi ctrl functions.
// you can refer to this website below to upload a config file to ESP32 Flash.
// https://randomnerdtutorials.com/install-esp32-filesystem-uploader-arduino-ide/
// libraries:
// #include <LittleFS.h>
// #include <WIFI.h>
// #include <ArduinoJson.h>
// you need to init Serial.
// bool InfoPrint = true;
// wifi config
// wifi mode on boot.
// 0: OFF (you need to use uart-command or upload a new wifiConfig.json to turn it on again)
// 1: AP (default mode as a brand new product)
// 2: STA
// 3: AP+STA (default mode after first wifi connection succeed)
byte WIFI_MODE_ON_BOOT = 1;
const char* sta_ssid = "none";
const char* sta_password = "none";
const char* ap_ssid = "UGV";
const char* ap_password = "12345678";
// true: change the WIFI_MODE_ON_BOOT to 3 when first STA mode succeed.
bool defaultModeToAPSTA = true;
// wifiConfig.yaml example:
// wifi_mode_on_boot:3
// sta_ssid:"WIFI_NAME"
// sta_ssid:"WIFI_PASSWORD"
// ap_ssid:"WIFI_NAME"
// ap_ssid:"WIFI_PASSWORD"
File wifiConfigYaml;
// other args:
unsigned long connectionStartTime;
unsigned long connectionTimeout = 15000;
byte WIFI_CURRENT_MODE = -1;
IPAddress localIP;
DynamicJsonDocument wifiDoc(256);
bool wifiConfigFound = false;
// update oled accroding to wifi settings.
void updateOledWifiInfo() {
switch(WIFI_CURRENT_MODE) {
case 0:
screenLine_0 = "AP: OFF";
screenLine_1 = "ST: OFF";
break;
case 1:
screenLine_0 = String("AP:") + ap_ssid;
screenLine_1 = "ST: OFF";
break;
case 2:
screenLine_0 = "AP: OFF";
screenLine_1 = String("ST:") + localIP.toString();
break;
case 3:
screenLine_0 = String("AP:") + ap_ssid;
screenLine_1 = String("ST:") + localIP.toString();
break;
}
oled_update();
}
// load the wifiConfig.json form Flash.
// the file name is wifiConfig.json in root path.
bool loadWifiConfig() {
wifiConfigYaml = LittleFS.open("/wifiConfig.json", "r");
if (wifiConfigYaml) {
if (InfoPrint == 1) {Serial.println("/wifiConfig.json load succeed.");}
String line = wifiConfigYaml.readStringUntil('\n');
// parse the YAML file using ArduinoJson.
deserializeJson(wifiDoc, line);
// read configuration values.
WIFI_MODE_ON_BOOT = wifiDoc["wifi_mode_on_boot"];
sta_ssid = wifiDoc["sta_ssid"];
sta_password = wifiDoc["sta_password"];
ap_ssid = wifiDoc["ap_ssid"];
ap_password = wifiDoc["ap_password"];
if (InfoPrint == 1) {
Serial.println(line);
}
wifiConfigYaml.close();
wifiConfigFound = true;
jsonInfoHttp.clear();
jsonInfoHttp["ip"] = "/wifiConfig.json load succeed.";
jsonInfoHttp["wifi_mode_on_boot"] = WIFI_MODE_ON_BOOT;
jsonInfoHttp["sta_ssid"] = sta_ssid;
jsonInfoHttp["sta_password"] = sta_password;
jsonInfoHttp["ap_ssid"] = ap_ssid;
jsonInfoHttp["ap_password"] = ap_password;
return true;
} else {
if (InfoPrint == 1) {Serial.println("cound not found wifiConfig.json.");}
wifiConfigFound = false;
return false;
}
}
// get the ip address.
IPAddress getIPAddress(byte inputMode) {
localIP = WiFi.localIP();
if (InfoPrint == 1) {
Serial.print("IP: ");
Serial.println(localIP.toString());
}
jsonInfoHttp.clear();
jsonInfoHttp["ip"] = localIP.toString();
return localIP;
}
// create a wifiConfig.json file
// from the args already be using.
bool createWifiConfigFileByStatus() {
if (WIFI_MODE_ON_BOOT != 0 || WIFI_MODE_ON_BOOT != -1){
wifiDoc.clear();
wifiDoc["wifi_mode_on_boot"] = WIFI_MODE_ON_BOOT;
wifiDoc["sta_ssid"] = sta_ssid;
wifiDoc["sta_password"] = sta_password;
wifiDoc["ap_ssid"] = ap_ssid;
wifiDoc["ap_password"] = ap_password;
File configFile = LittleFS.open("/wifiConfig.json", "w");
if (configFile) {
serializeJson(wifiDoc, configFile);
configFile.close();
if (InfoPrint == 1) {
Serial.println("/wifiConfig.json created.");
}
jsonInfoHttp.clear();
jsonInfoHttp["info"] = "/wifiConfig.json created.";
jsonInfoHttp["wifi_mode_on_boot"] = WIFI_MODE_ON_BOOT;
jsonInfoHttp["sta_ssid"] = sta_ssid;
jsonInfoHttp["sta_password"] = sta_password;
jsonInfoHttp["ap_ssid"] = ap_ssid;
jsonInfoHttp["ap_password"] = ap_password;
return true;
} else {
jsonInfoHttp.clear();
jsonInfoHttp["info"] = "/wifiConfig.json open failed.";
return false;
}
} else {
jsonInfoHttp.clear();
jsonInfoHttp["info"] = "not for this wifi_mode_on_boot.";
return false;
}
}
// set wifi as AP mode.
bool wifiModeAP(const char* input_ssid, const char* input_password) {
WiFi.disconnect();
if (InfoPrint == 1) {Serial.println("wifi mode on boot: AP");}
// WiFi.mode(WIFI_AP);
WiFi.mode(WIFI_AP_STA);
WiFi.softAP(input_ssid, input_password);
if (InfoPrint == 1) {
Serial.println("AP mode starts...");
Serial.print("SSID: ");
Serial.println(input_ssid);
Serial.print("Password: ");
Serial.println(input_password);
Serial.println("AP Address: 192.168.4.1");
}
WIFI_CURRENT_MODE = 1;
localIP = WiFi.localIP();
ap_ssid = input_ssid;
ap_password = input_password;
updateOledWifiInfo();
jsonInfoHttp.clear();
jsonInfoHttp["info"] = "AP mode starts";
jsonInfoHttp["ap_ssid"] = ap_ssid;
jsonInfoHttp["ap_password"] = ap_password;
return true;
}
// set wifi as STA mode.
bool wifiModeSTA(const char* input_ssid, const char* input_password) {
WiFi.disconnect();
if (InfoPrint == 1) {Serial.println("wifi mode on boot: STA");}
// WiFi.mode(WIFI_STA);
WiFi.mode(WIFI_AP_STA);
WiFi.begin(input_ssid, input_password);
connectionStartTime = millis();
if (InfoPrint == 1) {Serial.println("STA mode starts: connecting to ");
Serial.println(input_ssid);}
while (WiFi.status() != WL_CONNECTED) {
unsigned long currentTime = millis();
if (InfoPrint == 1) {Serial.print(".");}
delay(500);
if (currentTime - connectionStartTime >= connectionTimeout) {
WIFI_CURRENT_MODE = -1;
if (InfoPrint == 1) {Serial.println(".");Serial.println("STA connection timeout.");}
wifiModeAP(ap_ssid, ap_password);
updateOledWifiInfo();
jsonInfoHttp.clear();
jsonInfoHttp["info"] = "STA connection timeout.";
return false;
break;
}
}
if (InfoPrint == 1) {Serial.println(".");Serial.println("STA connection succeed.");}
WIFI_CURRENT_MODE = 2;
getIPAddress(WIFI_CURRENT_MODE);
sta_ssid = input_ssid;
sta_password = input_password;
jsonInfoHttp.clear();
jsonInfoHttp["info"] = "STA connection succeed.";
jsonInfoHttp["wifi_mode_on_boot"] = WIFI_MODE_ON_BOOT;
jsonInfoHttp["sta_ssid"] = sta_ssid;
jsonInfoHttp["sta_password"] = sta_password;
jsonInfoHttp["ap_ssid"] = ap_ssid;
jsonInfoHttp["ap_password"] = ap_password;
if (defaultModeToAPSTA && !wifiConfigFound) {
WIFI_MODE_ON_BOOT = 3;
if (InfoPrint == 1) {Serial.println("[default] wifi mode on boot: AP+STA");}
jsonInfoHttp["info"] = "[default] wifi mode on boot: AP+STA";
createWifiConfigFileByStatus();
}
updateOledWifiInfo();
return true;
}
// set wifi as AP+STA mode.
bool wifiModeAPSTA(const char* input_ap_ssid, const char* input_ap_password, const char* input_sta_ssid, const char* input_sta_password) {
WiFi.disconnect();
if (InfoPrint == 1) {Serial.println("wifi mode on boot: AP+STA");}
WiFi.mode(WIFI_AP_STA);
WiFi.softAP(input_ap_ssid, input_ap_password);
if (InfoPrint == 1) {
Serial.println("AP/AP+STA mode starts...");
Serial.print("AP SSID: ");
Serial.println(input_ap_ssid);
Serial.print("AP Password: ");
Serial.println(input_ap_password);
Serial.println("AP Address: 192.168.4.1");
}
ap_ssid = input_ap_ssid;
ap_password = input_ap_password;
WiFi.begin(input_sta_ssid, input_sta_password);
connectionStartTime = millis();
if (InfoPrint == 1) {Serial.print("STA/AP+STA mode starts: connecting to ");
Serial.println(input_sta_ssid);}
while (WiFi.status() != WL_CONNECTED) {
unsigned long currentTime = millis();
if (InfoPrint == 1) {Serial.print(".");}
delay(500);
if (currentTime - connectionStartTime >= connectionTimeout) {
WIFI_CURRENT_MODE = -1;
if (InfoPrint == 1) {Serial.println(".");Serial.println("STA connection timeout.");}
wifiModeAP(ap_ssid, ap_password);
updateOledWifiInfo();
jsonInfoHttp.clear();
jsonInfoHttp["info"] = "STA connection timeout.";
return false;
break;
}
}
if (InfoPrint == 1) {Serial.println("STA connection succeed.");}
WIFI_CURRENT_MODE = 3;
getIPAddress(WIFI_CURRENT_MODE);
sta_ssid = input_sta_ssid;
sta_password = input_sta_password;
if (defaultModeToAPSTA && !wifiConfigFound) {
WIFI_MODE_ON_BOOT = 3;
if (InfoPrint == 1) {Serial.println("[default] wifi mode on boot: AP+STA");}
createWifiConfigFileByStatus();
}
updateOledWifiInfo();
jsonInfoHttp.clear();
jsonInfoHttp["info"] = "STA connection succeed.";
jsonInfoHttp["wifi_mode_on_boot"] = WIFI_MODE_ON_BOOT;
jsonInfoHttp["sta_ssid"] = sta_ssid;
jsonInfoHttp["sta_password"] = sta_password;
jsonInfoHttp["ap_ssid"] = ap_ssid;
jsonInfoHttp["ap_password"] = ap_password;
return true;
}
// disconnect wifi.
void wifiStop() {
WiFi.disconnect();
WIFI_CURRENT_MODE = 0;
WiFi.mode(WIFI_AP_STA);
updateOledWifiInfo();
}
// wifi mode on boot starts.
bool wifiModeOnBoot() {
bool funcStatus = false;
switch(WIFI_MODE_ON_BOOT) {
case 0:
if (InfoPrint == 1) {
Serial.println("wifi mode on boot: OFF");
}
funcStatus = true;
WIFI_CURRENT_MODE = 0;
WiFi.mode(WIFI_AP_STA);
break;
case 1:
funcStatus = wifiModeAP(ap_ssid, ap_password);
break;
case 2:
funcStatus = wifiModeSTA(sta_ssid, sta_password);
break;
case 3:
funcStatus = wifiModeAPSTA(ap_ssid, ap_password, sta_ssid, sta_password);
break;
}
return funcStatus;
}
// change the WIFI_MODE_ON_BOOT.
void configWifiModeOnBoot(byte inputMode) {
WIFI_MODE_ON_BOOT = inputMode;
if (InfoPrint == 1) {
Serial.print("wifi_mode_on_boot: ");
Serial.println(WIFI_MODE_ON_BOOT);
}
createWifiConfigFileByStatus();
}
// create a wifiConfig.json file
// from the args input.
void createWifiConfigFileByInput(byte inputMode, const char* inputApSsid, const char* inputApPassword, const char* inputStaSsid, const char* inputStaPassword) {
WIFI_MODE_ON_BOOT = inputMode;
wifiModeAPSTA(inputApSsid, inputApPassword, inputStaSsid, inputStaPassword);
if (InfoPrint == 1) {
Serial.print("wifi_mode_on_boot: ");
Serial.println(WIFI_MODE_ON_BOOT);
}
createWifiConfigFileByStatus();
}
// wifi information feedback.
void wifiStatusFeedback() {
wifiDoc["ip"] = localIP.toString();
wifiDoc["rssi"] = WiFi.RSSI();
serializeJson(wifiDoc, Serial);
jsonInfoHttp.clear();
jsonInfoHttp["ip"] = wifiDoc["ip"];
jsonInfoHttp["rssi"] = wifiDoc["rssi"];
jsonInfoHttp["wifi_mode_on_boot"] = WIFI_MODE_ON_BOOT;
jsonInfoHttp["sta_ssid"] = sta_ssid;
jsonInfoHttp["sta_password"] = sta_password;
jsonInfoHttp["ap_ssid"] = ap_ssid;
jsonInfoHttp["ap_password"] = ap_password;
jsonInfoHttp["mac"] = thisMacStr;
}
// wifi init.
void initWifi() {
loadWifiConfig();
wifiModeOnBoot();
}