From e0bce862d735785ee6d8ab4e494924e500207a3b Mon Sep 17 00:00:00 2001 From: DeSmooth Date: Tue, 11 Jan 2022 18:42:49 +0800 Subject: [PATCH] first commit: added some comments, rename some files --- .gitignore | 5 + .vscode/extensions.json | 7 + README.md | 77 +++++++ include/README | 39 ++++ lib/README | 46 ++++ platformio.ini | 21 ++ src/Command.cpp | 29 +++ src/Command.h | 17 ++ src/Kalman.cpp | 93 ++++++++ src/Kalman.h | 59 ++++++ src/MPU6050.cpp | 84 ++++++++ src/MPU6050.h | 10 + src/main.cpp | 459 ++++++++++++++++++++++++++++++++++++++++ test/.gitignore | 5 + test/README | 11 + 15 files changed, 962 insertions(+) create mode 100644 .gitignore create mode 100644 .vscode/extensions.json create mode 100644 README.md create mode 100644 include/README create mode 100644 lib/README create mode 100644 platformio.ini create mode 100644 src/Command.cpp create mode 100644 src/Command.h create mode 100644 src/Kalman.cpp create mode 100644 src/Kalman.h create mode 100644 src/MPU6050.cpp create mode 100644 src/MPU6050.h create mode 100644 src/main.cpp create mode 100644 test/.gitignore create mode 100644 test/README diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..89cc49c --- /dev/null +++ b/.gitignore @@ -0,0 +1,5 @@ +.pio +.vscode/.browse.c_cpp.db* +.vscode/c_cpp_properties.json +.vscode/launch.json +.vscode/ipch diff --git a/.vscode/extensions.json b/.vscode/extensions.json new file mode 100644 index 0000000..e80666b --- /dev/null +++ b/.vscode/extensions.json @@ -0,0 +1,7 @@ +{ + // See http://go.microsoft.com/fwlink/?LinkId=827846 + // for the documentation about the extensions.json format + "recommendations": [ + "platformio.platformio-ide" + ] +} diff --git a/README.md b/README.md new file mode 100644 index 0000000..b9a6d08 --- /dev/null +++ b/README.md @@ -0,0 +1,77 @@ +# 自平衡莱洛三角形可充电版 + +基于 B站up 45555菌 [开源的自平衡莱洛三角形](s)所设计。此仓库维护我的一份使用 PlatformIO 环境的软件版本。我的[展示视频](https://www.bilibili.com/video/BV14a411q7EF) + + +> **在B站[“基于LQR控制器的自平衡莱洛三角形”](https://www.bilibili.com/video/BV19v411n7mN)基础上添加了充电模块** +主控芯片使用ESP32，并配置了调参上位机，可以很方便的通过wifi无线调参。无刷控制使用灯哥开源FOC。制作出一个方便复刻的自平衡莱洛三角形，在桌面上作为一个摆件还是非常不错的。[展示视频](https://www.bilibili.com/video/BV1b3411x7za) + + +| 说明 | 参数 | +| ---------------- |---------------------- | +| 莱洛三角形尺寸 | 100*100 mm | +|动量轮尺寸|80*80 mm| +|电池*3 |厚x长x宽:7.9mmx25mmx40mm| +| 输入电压 | 3.7v锂电池*3| +|充电电压| 5V 从Type-C口输入| +|充电芯片CS5095|5V输入,最大1.2A充电电流| +|串口芯片CH340C|需要打开开关才能下载| +| 主控芯片 | ESP-WROOM-32 | +|电机驱动芯片L6234PD|引脚:32, 33, 25, 22; 22为enable| +| AS5600 编码器 |SDA-23 SCL-5 芯片要离磁铁有2mm以上高度| +| MPU6050六轴传感器 | SDA-19 SCL-18 | + + +# 使用说明 + +**详细使用说明请参考源开源项目，此处仅记录我认为的部分重要内容。** + +比如让平衡角度为90度，则输入：TA90，并且会存入eeprom的位置0中 注：wifi发送**命令不能过快**，因为每次都会保存进eeprom，K参数没有保存到EEPROM所以可以使用滑条调整。 + +| 参数命令 | 说明 | +| ---------------- |---------------------- | +| TA | target_angle平衡角度 例如TA89.3 设置平衡角度89.3| +| SV | swing_up_voltage摇摆电压 左右摇摆的电压，越大越快到平衡态，但是过大会翻过头| +|SA|swing_up_angle摇摆角度 离平衡角度还有几度时候，切换到自平衡控制| +|VP1|速度环的PID的P，1是稳定在平衡角度之前的P值| +|VI1|速度环的PID的I，1是稳定在平衡角度之前的I值| +|VP2|速度环的PID的P，2是稳定后的P值| +|VI2|速度环的PID的I，2是稳定后的I值| +|K为LQR参数|第一个数字**1和2**是电压控制稳定前和后**3和4**是速度控制稳定前和后，第二个数字如下| +|K1**1**|LQR的参数1：角度差值| +|K1**2**|LQR的参数2：左右倾倒加速度| +|K1**3**|LQR的参数3：当前速度| + +LQR算法解释：当三角形向←倾斜时，需要产生向→的力回正。 + +在电压控制下：回正力F直接和输出电压值挂钩，向←倾斜，电机提供正电压V动量轮向**左加速转动**，产生向右的力F。 + +`期望电压 = 角度差值x参数1 + 左右倾倒加速度x参数2 + 当前速度x参数3` + +在速度控制下：回正力F和动量轮转速加速度a有关，F = ma，向←倾斜，电机需要向**左加速转动**，产生向右的力F。 + +`期望速度 = 角度差值x参数1 + 左右倾倒加速度x参数2 + 当前速度x参数3` + +两者区别：电压和速度控制都可以实现平衡，但因为使用simpleFOC控制电机转速无法无限加速，本电机实验最高转速180转，电压到上限12v。 + +使用电压控制会遇到，电机一下子就到了转速上限，就不能提供稳定的力F，参数调起来比较困难。 + +速度控制可以在偏差变大的时候，根据期望速度产生最大电压12v，并且不会超过电机最高转速。 +# 硬件设计 + +[莱洛三角形PCB](https://oshwhub.com/45coll/zi-ping-heng-di-lai-luo-san-jiao_10-10-ban-ben) + +[动量轮](https://oshwhub.com/45coll/lai-luo-san-jiao-dong-liang-lun) + +充电电路是立创广场开源的CS5095充电方案。 +1. 原作者：基于LQR控制器的自平衡莱洛三角形[BV19v411n7mN](https://www.bilibili.com/video/BV19v411n7mN) +2. 灯哥开源FOC [https://gitee.com/ream_d/Deng-s-foc-controller](https://gitee.com/ream_d/Deng-s-foc-controller) +3. 充电芯片电路[https://oshwhub.com/Aknice/cs5095e-san-jie-li-dian-chi-sheng-ya-chong-dian-dian-lu](https://oshwhub.com/Aknice/cs5095e-san-jie-li-dian-chi-sheng-ya-chong-dian-dian-lu) + +# 可复用的代码 + +1. Arduino的程序中的command.h、command.cpp可以支持任意的字符串输入。在其他项目中一样可以用，无论是wifi接收到的字符串数据或者是串口的字符串数据。 + +2. GUI上位机可以在其他wifi项目中可以继续使用，用来调参还是很方便。 + + diff --git a/include/README b/include/README new file mode 100644 index 0000000..194dcd4 --- /dev/null +++ b/include/README @@ -0,0 +1,39 @@ + +This directory is intended for project header files. + +A header file is a file containing C declarations and macro definitions +to be shared between several project source files. You request the use of a +header file in your project source file (C, C++, etc) located in `src` folder +by including it, with the C preprocessing directive `#include'. + +```src/main.c + +#include "header.h" + +int main (void) +{ + ... +} +``` + +Including a header file produces the same results as copying the header file +into each source file that needs it. Such copying would be time-consuming +and error-prone. With a header file, the related declarations appear +in only one place. If they need to be changed, they can be changed in one +place, and programs that include the header file will automatically use the +new version when next recompiled. The header file eliminates the labor of +finding and changing all the copies as well as the risk that a failure to +find one copy will result in inconsistencies within a program. + +In C, the usual convention is to give header files names that end with `.h'. +It is most portable to use only letters, digits, dashes, and underscores in +header file names, and at most one dot. + +Read more about using header files in official GCC documentation: + +* Include Syntax +* Include Operation +* Once-Only Headers +* Computed Includes + +https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html diff --git a/lib/README b/lib/README new file mode 100644 index 0000000..6debab1 --- /dev/null +++ b/lib/README @@ -0,0 +1,46 @@ + +This directory is intended for project specific (private) libraries. +PlatformIO will compile them to static libraries and link into executable file. + +The source code of each library should be placed in a an own separate directory +("lib/your_library_name/[here are source files]"). + +For example, see a structure of the following two libraries `Foo` and `Bar`: + +|--lib +| | +| |--Bar +| | |--docs +| | |--examples +| | |--src +| | |- Bar.c +| | |- Bar.h +| | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html +| | +| |--Foo +| | |- Foo.c +| | |- Foo.h +| | +| |- README --> THIS FILE +| +|- platformio.ini +|--src + |- main.c + +and a contents of `src/main.c`: +``` +#include +#include + +int main (void) +{ + ... +} + +``` + +PlatformIO Library Dependency Finder will find automatically dependent +libraries scanning project source files. + +More information about PlatformIO Library Dependency Finder +- https://docs.platformio.org/page/librarymanager/ldf.html diff --git a/platformio.ini b/platformio.ini new file mode 100644 index 0000000..cd1e397 --- /dev/null +++ b/platformio.ini @@ -0,0 +1,21 @@ +; PlatformIO Project Configuration File +; +; Build options: build flags, source filter +; Upload options: custom upload port, speed and extra flags +; Library options: dependencies, extra library storages +; Advanced options: extra scripting +; +; Please visit documentation for the other options and examples +; https://docs.platformio.org/page/projectconf.html + +[env:esp32dev] +platform = espressif32 +board = esp32dev +framework = arduino +lib_deps = + askuric/Simple FOC@2.1.1 + SPI + Wire + +lib_archive = false +monitor_speed = 115200 \ No newline at end of file diff --git a/src/Command.cpp b/src/Command.cpp new file mode 100644 index 0000000..5ce3d2f --- /dev/null +++ b/src/Command.cpp @@ -0,0 +1,29 @@ +#include "Command.h" + + +void Command::run(char* str){ + for(int i=0; i < call_count; i++){ + if(isSentinel(call_ids[i],str)){ // case : call_ids = "T2" str = "T215.15" + call_list[i](str+strlen(call_ids[i])); // get 15.15 input function + break; + } + } +} +void Command::add(char* id, CommandCallback onCommand){ + call_list[call_count] = onCommand; + call_ids[call_count] = id; + call_count++; +} +void Command::scalar(float* value, char* user_cmd){ + *value = atof(user_cmd); +} +bool Command::isSentinel(char* ch,char* str) +{ + char s[strlen(ch)+1]; + strncpy(s,str,strlen(ch)); + s[strlen(ch)] = '\0'; //strncpy need add end '\0' + if(strcmp(ch, s) == 0) + return true; + else + return false; +} diff --git a/src/Command.h b/src/Command.h new file mode 100644 index 0000000..20e2fe5 --- /dev/null +++ b/src/Command.h @@ -0,0 +1,17 @@ +#include +// callback function pointer definiton +typedef void (* CommandCallback)(char*); //!< command callback function pointer +class Command +{ + public: + void add(char* id , CommandCallback onCommand); + void run(char* str); + void scalar(float* value, char* user_cmd); + bool isSentinel(char* ch,char* str); + private: + // Subscribed command callback variables + CommandCallback call_list[20];//!< array of command callback pointers - 20 is an arbitrary number + char* call_ids[20]; //!< added callback commands + int call_count;//!< number callbacks that are subscribed + +}; diff --git a/src/Kalman.cpp b/src/Kalman.cpp new file mode 100644 index 0000000..80c7dec --- /dev/null +++ b/src/Kalman.cpp @@ -0,0 +1,93 @@ +/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved. + + This software may be distributed and modified under the terms of the GNU + General Public License version 2 (GPL2) as published by the Free Software + Foundation and appearing in the file GPL2.TXT included in the packaging of + this file. Please note that GPL2 Section 2[b] requires that all works based + on this software must also be made publicly available under the terms of + the GPL2 ("Copyleft"). + + Contact information + ------------------- + + Kristian Lauszus, TKJ Electronics + Web : http://www.tkjelectronics.com + e-mail : kristianl@tkjelectronics.com + */ + +#include "Kalman.h" + +Kalman::Kalman() { + /* We will set the variables like so, these can also be tuned by the user */ + Q_angle = 0.001f; + Q_bias = 0.003f; + R_measure = 0.03f; + + angle = 0.0f; // Reset the angle + bias = 0.0f; // Reset bias + + P[0][0] = 0.0f; // Since we assume that the bias is 0 and we know the starting angle (use setAngle), the error covariance matrix is set like so - see: http://en.wikipedia.org/wiki/Kalman_filter#Example_application.2C_technical + P[0][1] = 0.0f; + P[1][0] = 0.0f; + P[1][1] = 0.0f; +}; + +// The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds +float Kalman::getAngle(float newAngle, float newRate, float dt) { + // KasBot V2 - Kalman filter module - http://www.x-firm.com/?page_id=145 + // Modified by Kristian Lauszus + // See my blog post for more information: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it + + // Discrete Kalman filter time update equations - Time Update ("Predict") + // Update xhat - Project the state ahead + /* Step 1 */ + rate = newRate - bias; + angle += dt * rate; + + // Update estimation error covariance - Project the error covariance ahead + /* Step 2 */ + P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle); + P[0][1] -= dt * P[1][1]; + P[1][0] -= dt * P[1][1]; + P[1][1] += Q_bias * dt; + + // Discrete Kalman filter measurement update equations - Measurement Update ("Correct") + // Calculate Kalman gain - Compute the Kalman gain + /* Step 4 */ + float S = P[0][0] + R_measure; // Estimate error + /* Step 5 */ + float K[2]; // Kalman gain - This is a 2x1 vector + K[0] = P[0][0] / S; + K[1] = P[1][0] / S; + + // Calculate angle and bias - Update estimate with measurement zk (newAngle) + /* Step 3 */ + float y = newAngle - angle; // Angle difference + /* Step 6 */ + angle += K[0] * y; + bias += K[1] * y; + + // Calculate estimation error covariance - Update the error covariance + /* Step 7 */ + float P00_temp = P[0][0]; + float P01_temp = P[0][1]; + + P[0][0] -= K[0] * P00_temp; + P[0][1] -= K[0] * P01_temp; + P[1][0] -= K[1] * P00_temp; + P[1][1] -= K[1] * P01_temp; + + return angle; +}; + +void Kalman::setAngle(float angle) { this->angle = angle; }; // Used to set angle, this should be set as the starting angle +float Kalman::getRate() { return this->rate; }; // Return the unbiased rate + +/* These are used to tune the Kalman filter */ +void Kalman::setQangle(float Q_angle) { this->Q_angle = Q_angle; }; +void Kalman::setQbias(float Q_bias) { this->Q_bias = Q_bias; }; +void Kalman::setRmeasure(float R_measure) { this->R_measure = R_measure; }; + +float Kalman::getQangle() { return this->Q_angle; }; +float Kalman::getQbias() { return this->Q_bias; }; +float Kalman::getRmeasure() { return this->R_measure; }; diff --git a/src/Kalman.h b/src/Kalman.h new file mode 100644 index 0000000..7de545f --- /dev/null +++ b/src/Kalman.h @@ -0,0 +1,59 @@ +/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved. + + This software may be distributed and modified under the terms of the GNU + General Public License version 2 (GPL2) as published by the Free Software + Foundation and appearing in the file GPL2.TXT included in the packaging of + this file. Please note that GPL2 Section 2[b] requires that all works based + on this software must also be made publicly available under the terms of + the GPL2 ("Copyleft"). + + Contact information + ------------------- + + Kristian Lauszus, TKJ Electronics + Web : http://www.tkjelectronics.com + e-mail : kristianl@tkjelectronics.com + */ + +#ifndef _Kalman_h_ +#define _Kalman_h_ + +class Kalman { +public: + Kalman(); + + // The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds + float getAngle(float newAngle, float newRate, float dt); + + void setAngle(float angle); // Used to set angle, this should be set as the starting angle + float getRate(); // Return the unbiased rate + + /* These are used to tune the Kalman filter */ + void setQangle(float Q_angle); + /** + * setQbias(float Q_bias) + * Default value (0.003f) is in Kalman.cpp. + * Raise this to follow input more closely, + * lower this to smooth result of kalman filter. + */ + void setQbias(float Q_bias); + void setRmeasure(float R_measure); + + float getQangle(); + float getQbias(); + float getRmeasure(); + +private: + /* Kalman filter variables */ + float Q_angle; // Process noise variance for the accelerometer + float Q_bias; // Process noise variance for the gyro bias + float R_measure; // Measurement noise variance - this is actually the variance of the measurement noise + + float angle; // The angle calculated by the Kalman filter - part of the 2x1 state vector + float bias; // The gyro bias calculated by the Kalman filter - part of the 2x1 state vector + float rate; // Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate + + float P[2][2]; // Error covariance matrix - This is a 2x2 matrix +}; + +#endif diff --git a/src/MPU6050.cpp b/src/MPU6050.cpp new file mode 100644 index 0000000..f8438bf --- /dev/null +++ b/src/MPU6050.cpp @@ -0,0 +1,84 @@ +/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved. + + This software may be distributed and modified under the terms of the GNU + General Public License version 2 (GPL2) as published by the Free Software + Foundation and appearing in the file GPL2.TXT included in the packaging of + this file. Please note that GPL2 Section 2[b] requires that all works based + on this software must also be made publicly available under the terms of + the GPL2 ("Copyleft"). + + Contact information + ------------------- + + Kristian Lauszus, TKJ Electronics + Web : http://www.tkjelectronics.com + e-mail : kristianl@tkjelectronics.com + */ + +#include "MPU6050.h" +#include +#include + +const uint8_t IMUAddress = 0x68; // AD0 is logic low on the PCB +const uint16_t I2C_TIMEOUT = 1000; // Used to check for errors in I2C communication + + +/* + * mpu6050加速度转换为角度 + * acc2rotation(ax, ay) + * acc2rotation(az, ay) +*/ +double acc2rotation(double x, double y) +{ + if(y < 0) { + return atan(x / y) / 1.570796 * 90 + 180; + } else if (x < 0) { + return (atan(x / y) / 1.570796 * 90 + 360); + } else { + return (atan(x / y) / 1.570796 * 90); + } +} + +uint8_t i2cWrite(uint8_t registerAddress, uint8_t data, bool sendStop) { + return i2cWrite(registerAddress, &data, 1, sendStop); // Returns 0 on success +} + +uint8_t i2cWrite(uint8_t registerAddress, uint8_t *data, uint8_t length, bool sendStop) { + Wire.beginTransmission(IMUAddress); + Wire.write(registerAddress); + Wire.write(data, length); + uint8_t rcode = Wire.endTransmission(sendStop); // Returns 0 on success + if (rcode) { + Serial.print(F("i2cWrite failed: ")); + Serial.println(rcode); + } + return rcode; // See: http://arduino.cc/en/Reference/WireEndTransmission +} + +uint8_t i2cRead(uint8_t registerAddress, uint8_t *data, uint8_t nbytes) { + uint32_t timeOutTimer; + Wire.beginTransmission(IMUAddress); + Wire.write(registerAddress); + uint8_t rcode = Wire.endTransmission(false); // Don't release the bus + if (rcode) { + Serial.print(F("i2cRead failed: ")); + Serial.println(rcode); + return rcode; // See: http://arduino.cc/en/Reference/WireEndTransmission + } + Wire.requestFrom(IMUAddress, nbytes, (uint8_t)true); // Send a repeated start and then release the bus after reading + for (uint8_t i = 0; i < nbytes; i++) { + if (Wire.available()) + data[i] = Wire.read(); + else { + timeOutTimer = micros(); + while (((micros() - timeOutTimer) < I2C_TIMEOUT) && !Wire.available()); + if (Wire.available()) + data[i] = Wire.read(); + else { + Serial.println(F("i2cRead timeout")); + return 5; // This error value is not already taken by endTransmission + } + } + } + return 0; // Success +} diff --git a/src/MPU6050.h b/src/MPU6050.h new file mode 100644 index 0000000..333fe3c --- /dev/null +++ b/src/MPU6050.h @@ -0,0 +1,10 @@ +#ifndef __MT_I2C_H__ +#define __MT_I2C_H__ + +#include +uint8_t i2cWrite(uint8_t registerAddress, uint8_t data, bool sendStop); +uint8_t i2cWrite(uint8_t registerAddress, uint8_t *data, uint8_t length, bool sendStop); +uint8_t i2cRead(uint8_t registerAddress, uint8_t *data, uint8_t nbytes); +double acc2rotation(double x, double y); + +#endif // __MT_I2C_H__ diff --git a/src/main.cpp b/src/main.cpp new file mode 100644 index 0000000..465c688 --- /dev/null +++ b/src/main.cpp @@ -0,0 +1,459 @@ + /** +arduino开发环境-灯哥开源FOChttps://gitee.com/ream_d/Deng-s-foc-controller，并安装Kalman。 +FOC引脚32, 33, 25, 22 22为enable +AS5600霍尔传感器 SDA-23 SCL-5 MPU6050六轴传感器 SDA-19 SCL-18 +本程序有两种平衡方式， FLAG_V为1时使用电压控制，为0时使用速度控制。电压控制时LQR参数使用K1和K2，速度控制时LQR参数使用K3和K4 +在wifi上位机窗口中输入：TA+角度，就可以修改平衡角度 +比如让平衡角度为90度，则输入：TA90，并且会存入eeprom的位置0中 注：wifi发送命令不能过快，因为每次都会保存进eeprom + +在使用自己的电机时，请一定记得修改默认极对数，即 BLDCMotor(5) 中的值，设置为自己的极对数数字，磁铁数量/2。 + +程序默认设置的供电电压为 12V,用其他电压供电请记得修改 voltage_power_supply , voltage_limit 变量中的值 + +默认PID针对的电机是 GB2204 ，使用自己的电机需要修改PID参数，才能实现更好效果 + */ +#include +#include "Command.h" +#include +#include //引用以使用异步UDP +#include "Kalman.h" // Source: https://github.com/TKJElectronics/KalmanFilter +#include "EEPROM.h" +#include "MPU6050.h" + +Kalman kalmanZ; +#define gyroZ_OFF -0.19 +#define FLAG_V 0 +#define DUBUG 1 +/* ----IMU Data---- */ + +double accX, accY, accZ; +double gyroX, gyroY, gyroZ; +int16_t tempRaw; +bool stable = 0; +uint32_t last_unstable_time; + +double gyroZangle; // Angle calculate using the gyro only +double compAngleZ; // Calculated angle using a complementary filter +double kalAngleZ; // 使用卡尔曼滤波计算角度 + +uint32_t timer; +uint8_t i2cData[14]; // Buffer for I2C data +/* ----FOC Data---- */ + +// driver instance +double acc2rotation(double x, double y); +float constrainAngle(float x); +float controllerLQR(float p_angle, float p_vel, float m_vel); + +const char *ssid = "esp32"; +const char *password = "12345678"; + +bool wifi_flag = 0; +AsyncUDP udp; //创建UDP对象 +unsigned int localUdpPort = 2333; //本地端口号 +void wifi_print(char * s,double num); + +MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C); +TwoWire I2Ctwo = TwoWire(1); +LowPassFilter lpf_throttle{0.00}; + +// 倒立摆参数：电压控制使用 K1 K2, 速度控制使用 k3 k4 +float LQR_K1_1 = 4; //摇摆到平衡 +float LQR_K1_2 = 1.5; +float LQR_K1_3 = 0.30; + +float LQR_K2_1 = 3.49; //平衡态 +float LQR_K2_2 = 0.26; +float LQR_K2_3 = 0.15; + +float LQR_K3_1 = 10; //摇摆到平衡 +float LQR_K3_2 = 1.7; // +float LQR_K3_3 = 1.75; // + +float LQR_K4_1 = 2.4; //平衡态 +float LQR_K4_2 = 1.5; +float LQR_K4_3 = 1.42; + +//电机参数 +BLDCMotor motor = BLDCMotor(5); +BLDCDriver3PWM driver = BLDCDriver3PWM(32, 33, 25, 22); +float target_velocity = 0; +float target_angle = 89.5; +float target_voltage = 0; +float swing_up_voltage = 1.8; +float swing_up_angle = 20; +float v_i_1 = 20; +float v_p_1 = 0.5; +float v_i_2 = 10; +float v_p_2 = 0.2; +//命令设置 +Command comm; +bool Motor_enable_flag = 0; +void do_TA(char* cmd) { comm.scalar(&target_angle, cmd);EEPROM.writeFloat(0, target_angle); } +void do_SV(char* cmd) { comm.scalar(&swing_up_voltage, cmd); EEPROM.writeFloat(4, swing_up_voltage); } +void do_SA(char* cmd) { comm.scalar(&swing_up_angle, cmd);EEPROM.writeFloat(8, swing_up_angle); } + +void do_START(char* cmd) { wifi_flag = !wifi_flag; } +void do_MOTOR(char* cmd) +{ + if(Motor_enable_flag) + motor.enable(); + else + motor.disable(); + Motor_enable_flag = !Motor_enable_flag; +} +#if FLAG_V +void do_K11(char* cmd) { comm.scalar(&LQR_K1_1, cmd); } +void do_K12(char* cmd) { comm.scalar(&LQR_K1_2, cmd); } +void do_K13(char* cmd) { comm.scalar(&LQR_K1_3, cmd); } +void do_K21(char* cmd) { comm.scalar(&LQR_K2_1, cmd); } +void do_K22(char* cmd) { comm.scalar(&LQR_K2_2, cmd); } +void do_K23(char* cmd) { comm.scalar(&LQR_K2_3, cmd); } +#else +void do_vp1(char* cmd) { comm.scalar(&v_p_1, cmd); EEPROM.writeFloat(12, v_p_1);} +void do_vi1(char* cmd) { comm.scalar(&v_i_1, cmd);EEPROM.writeFloat(16, v_i_1); } +void do_vp2(char* cmd) { comm.scalar(&v_p_2, cmd); EEPROM.writeFloat(20, v_p_2);} +void do_vi2(char* cmd) { comm.scalar(&v_i_2, cmd);EEPROM.writeFloat(24, v_i_2); } +void do_tv(char* cmd) { comm.scalar(&target_velocity, cmd); } // 目标速度 +// LRQ 控制参数没有写到 EEPROM 中 +void do_K31(char* cmd) { comm.scalar(&LQR_K3_1, cmd); } +void do_K32(char* cmd) { comm.scalar(&LQR_K3_2, cmd); } +void do_K33(char* cmd) { comm.scalar(&LQR_K3_3, cmd); } +void do_K41(char* cmd) { comm.scalar(&LQR_K4_1, cmd); } +void do_K42(char* cmd) { comm.scalar(&LQR_K4_2, cmd); } +void do_K43(char* cmd) { comm.scalar(&LQR_K4_3, cmd); } +#endif + + +void onPacketCallBack(AsyncUDPPacket packet) +{ + char* da; + da= (char*)(packet.data()); + Serial.println(da); + comm.run(da); + EEPROM.commit(); +// packet.print("reply data"); +} +// instantiate the commander +void setup() { + Serial.begin(115200); + if (!EEPROM.begin(1000)) { + Serial.println("Failed to initialise EEPROM"); + Serial.println("Restarting..."); + delay(1000); + ESP.restart(); + } + // eeprom 读取 + float nan = EEPROM.readFloat(0); + if(isnan(nan)) + { + Serial.println("frist write"); + EEPROM.writeFloat(0, target_angle); delay(10); EEPROM.commit(); + EEPROM.writeFloat(4, swing_up_voltage); delay(10); EEPROM.commit(); + EEPROM.writeFloat(8, swing_up_angle); delay(10); EEPROM.commit(); + EEPROM.writeFloat(12, v_p_1); delay(10); EEPROM.commit(); + EEPROM.writeFloat(16, v_i_1); delay(10); EEPROM.commit(); + EEPROM.writeFloat(20, v_p_2); delay(10); EEPROM.commit(); + EEPROM.writeFloat(24, v_i_2); delay(10); EEPROM.commit(); + } + else + { + target_angle = EEPROM.readFloat(0); + swing_up_voltage = EEPROM.readFloat(4); + swing_up_angle = EEPROM.readFloat(8); + v_p_1 = EEPROM.readFloat(12); + v_i_1 = EEPROM.readFloat(16); + v_p_2 = EEPROM.readFloat(20); + v_i_2 = EEPROM.readFloat(24); + motor.PID_velocity.P = v_p_1; // 启动时使用 p1 + motor.PID_velocity.I = v_i_1; + } + //命令设置 + comm.add("TA",do_TA); + comm.add("START",do_START); + comm.add("MOTOR",do_MOTOR); + comm.add("SV",do_SV); + comm.add("SA",do_SA); + +#if FLAG_V //电压模式 + comm.add("K11",do_K11); + comm.add("K12",do_K12); + comm.add("K13",do_K13); + comm.add("K21",do_K21); + comm.add("K22",do_K22); + comm.add("K23",do_K23); +#else //速度模式 + comm.add("VP1",do_vp1); + comm.add("VI1",do_vi1); + comm.add("VP2",do_vp2); + comm.add("VI2",do_vi2); + comm.add("TV",do_tv); + comm.add("K31",do_K31); + comm.add("K32",do_K32); + comm.add("K33",do_K33); + comm.add("K41",do_K41); + comm.add("K42",do_K42); + comm.add("K43",do_K43); +#endif + // kalman mpu6050 init + Wire.begin(19, 18,400000);// Set I2C frequency to 400kHz + i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz + i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling + i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s + i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g + + while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at once + while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode + + while (i2cRead(0x75, i2cData, 1)); + if (i2cData[0] != 0x68) + { // Read "WHO_AM_I" register + Serial.print(F("Error reading sensor")); + while (1); + } + + delay(100); // Wait for sensor to stabilize + + /* Set kalman and gyro starting angle */ + while (i2cRead(0x3B, i2cData, 6)); + accX = (int16_t)((i2cData[0] << 8) | i2cData[1]); + accY = (int16_t)((i2cData[2] << 8) | i2cData[3]); + accZ = (int16_t)((i2cData[4] << 8) | i2cData[5]); + double pitch = acc2rotation(accX, accY); + + kalmanZ.setAngle(pitch); + gyroZangle = pitch; + + timer = micros(); + Serial.println("kalman mpu6050 init"); + + //wifi初始化 + WiFi.mode(WIFI_AP); + //启动AP + while(!WiFi.softAP(ssid, password)); + Serial.println("AP启动成功"); + //等待udp监听设置成功 + while (!udp.listen(localUdpPort)); + + udp.onPacket(onPacketCallBack); //注册收到数据包事件 + + I2Ctwo.begin(23, 5, 400000); //SDA,SCL + sensor.init(&I2Ctwo); + + //连接motor对象与传感器对象 + motor.linkSensor(&sensor); + + //供电电压设置 [V] + driver.voltage_power_supply = 12; + driver.init(); + + //连接电机和driver对象 + motor.linkDriver(&driver); + + //FOC模型选择 + motor.foc_modulation = FOCModulationType::SpaceVectorPWM; + + //运动控制模式设置 +#if FLAG_V + motor.controller = MotionControlType::torque; +#else + motor.controller = MotionControlType::velocity; + //速度PI环设置 + motor.PID_velocity.P = 0.5; + motor.PID_velocity.I = 20; +#endif + + + //最大电机限制电机 + motor.voltage_limit = 12; + + //速度低通滤波时间常数 + motor.LPF_velocity.Tf = 0.01; + + //设置最大速度限制 + motor.velocity_limit = 40; + + motor.useMonitoring(Serial); + + //初始化电机 + motor.init(); + + //初始化 FOC + motor.initFOC(); + + Serial.println(F("Motor ready.")); + Serial.println(F("Set the target velocity using serial terminal:")); + + digitalWrite(22,LOW); // 默认停止电机 +} +char buf[255]; +long loop_count = 0; +double last_pitch; + +void loop() { + + motor.loopFOC(); +// loop_count++ == 10 +// loop_count = 0; + while(i2cRead(0x3B, i2cData, 14)); + + accX = (int16_t)((i2cData[0] << 8) | i2cData[1]); + accY = (int16_t)((i2cData[2] << 8) | i2cData[3]); + accZ = (int16_t)((i2cData[4] << 8) | i2cData[5]); + tempRaw = (int16_t)((i2cData[6] << 8) | i2cData[7]); + gyroX = (int16_t)((i2cData[8] << 8) | i2cData[9]); + gyroY = (int16_t)((i2cData[10] << 8) | i2cData[11]); + gyroZ = (int16_t)((i2cData[12] << 8) | i2cData[13]); // z 轴角速度 rad/s + + double dt = (double)(micros() - timer) / 1000000; // Calculate delta time + timer = micros(); + + double pitch = acc2rotation(accX, accY); + double gyroZrate = gyroZ / 131.0; // Convert to deg/s 弧度每秒转换为度每秒 + if(abs(pitch-last_pitch)>100) + kalmanZ.setAngle(pitch); + + kalAngleZ = kalmanZ.getAngle(pitch, gyroZrate + gyroZ_OFF, dt); + last_pitch = pitch; + gyroZangle += (gyroZrate + gyroZ_OFF) * dt; + compAngleZ = 0.93 * (compAngleZ + (gyroZrate + gyroZ_OFF) * dt) + 0.07 * pitch; // 角度补偿 + + // Reset the gyro angle when it has drifted too much + if(gyroZangle < -180 || gyroZangle > 180) + gyroZangle = kalAngleZ; + + // 对卡尔曼的值(0~360)以120取模，限制到0~120，从而控制出三个面的直立 + float pendulum_angle = constrainAngle(fmod(kalAngleZ,120) - target_angle); +// FLAG_V为1时使用电压控制，为0时候速度控制 +#if FLAG_V + if(abs(pendulum_angle) < swing_up_angle) // if angle small enough stabilize 0.5~30°,1.5~90° + { + target_voltage = controllerLQR(pendulum_angle, gyroZrate, motor.shaftVelocity()); + // limit the voltage set to the motor + if(abs(target_voltage) > motor.voltage_limit * 0.7) + target_voltage = _sign(target_voltage) * motor.voltage_limit * 0.7; + } + else // else do swing-up + { // sets 1.5V to the motor in order to swing up + target_voltage = -_sign(gyroZrate) * swing_up_voltage; + } + + // set the target voltage to the motor + if (accZ < -13000 && ((accX * accX + accY * accY) > (14000 * 14000))) { + motor.move(0); + } else { + motor.move(lpf_throttle(target_voltage)); + } + +#else + if (abs(pendulum_angle) < swing_up_angle) // if angle small enough stabilize 0.5~30°,1.5~90° + { + target_velocity = controllerLQR(pendulum_angle, gyroZrate, motor.shaftVelocity()); + if (abs(target_velocity) > 140) + target_velocity = _sign(target_velocity) * 140; + + motor.controller = MotionControlType::velocity; + motor.move(target_velocity); + } else { + // else do swing-up + // sets 1.5V to the motor in order to swing up + motor.controller = MotionControlType::torque; + target_voltage = -_sign(gyroZrate) * swing_up_voltage; + motor.move(target_voltage); + } +#endif + +#if DUBUG + // 串口调试打印 + Serial.print(pitch);Serial.print("\t"); + Serial.print(kalAngleZ);Serial.print("\t"); + Serial.print("\r\n"); +#endif + + //可以使用该方法广播信息 + if(wifi_flag) + { + memset(buf, 0, strlen(buf)); + + wifi_print("v", motor.shaftVelocity()); // 电机转速 + wifi_print("vq", motor.voltage.q); // 电机的期望电压 + wifi_print("p", pendulum_angle); // 期望角度和真实角度的差值 + wifi_print("t", target_angle); // 目标角度 + wifi_print("k", kalAngleZ); // 卡尔曼滤波后的角度 + wifi_print("g", gyroZrate); // z轴的加速度 + + udp.writeTo((const unsigned char*)buf, strlen(buf), IPAddress(192,168,4,2), localUdpPort); //广播数据 + } +} + + +// function constraining the angle in between -60~60 +// 本来是 -targetangle~120 +float constrainAngle(float x) +{ + float a = 0; + if(x < 0) { + a = 120 + x; + if(a < abs(x)) + return a; + } + return x; +} +// LQR stabilization controller functions +// calculating the voltage that needs to be set to the motor in order to stabilize the pendulum +float controllerLQR(float p_angle, float p_vel, float m_vel) +{ + // if angle controllable + // calculate the control law + // LQR controller u = k*x + // - k = [40, 7, 0.3] + // - k = [13.3, 21, 0.3] + // - x = [pendulum angle, pendulum velocity, motor velocity]' + + if(abs(p_angle) > 2.5) { + last_unstable_time = millis(); + if(stable) { + target_angle = EEPROM.readFloat(0); + stable = 0; + } + } + if((millis() - last_unstable_time) > 1000&&!stable) { + target_angle = target_angle+p_angle; + stable = 1; + } + + //Serial.println(stable); + float u; +#if FLAG_V + if (!stable) { + u = LQR_K1_1 * p_angle + LQR_K1_2 * p_vel + LQR_K1_3 * m_vel; + } else { + //u = LQR_K1 * p_angle + LQR_K2 * p_vel + LQR_K3 * m_vel; + u = LQR_K2_1 * p_angle + LQR_K2_2 * p_vel + LQR_K2_3 * m_vel; + } +#else + if(!stable) { + motor.PID_velocity.P = v_p_1; + motor.PID_velocity.I = v_i_1; + u = LQR_K3_1 * p_angle + LQR_K3_2 * p_vel + LQR_K3_3 * m_vel; + } else { + motor.PID_velocity.P = v_p_2; + motor.PID_velocity.I = v_i_2; + //u = LQR_K1 * p_angle + LQR_K2 * p_vel + LQR_K3 * m_vel; + u = LQR_K4_1 * p_angle + LQR_K4_2 * p_vel + LQR_K4_3 * m_vel; + } +#endif + + return u; +} + +void wifi_print(char * s,double num) +{ + char str[255]; + char n[255]; + sprintf(n, "%.2f",num); + strcpy(str,s); + strcat(str, n); + strcat(buf+strlen(buf), str); + strcat(buf, ",\0"); +} diff --git a/test/.gitignore b/test/.gitignore new file mode 100644 index 0000000..89cc49c --- /dev/null +++ b/test/.gitignore @@ -0,0 +1,5 @@ +.pio +.vscode/.browse.c_cpp.db* +.vscode/c_cpp_properties.json +.vscode/launch.json +.vscode/ipch diff --git a/test/README b/test/README new file mode 100644 index 0000000..b94d089 --- /dev/null +++ b/test/README @@ -0,0 +1,11 @@ + +This directory is intended for PlatformIO Unit Testing and project tests. + +Unit Testing is a software testing method by which individual units of +source code, sets of one or more MCU program modules together with associated +control data, usage procedures, and operating procedures, are tested to +determine whether they are fit for use. Unit testing finds problems early +in the development cycle. + +More information about PlatformIO Unit Testing: +- https://docs.platformio.org/page/plus/unit-testing.html