first commit: added some comments, rename some files

.gitignore

@@ -0,0 +1,5 @@
+.pio
+.vscode/.browse.c_cpp.db*
+.vscode/c_cpp_properties.json
+.vscode/launch.json
+.vscode/ipch

.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"
+    ]
+}

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项目中可以继续使用，用来调参还是很方便。
+
+

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

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 <Foo.h>
+#include <Bar.h>
+
+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

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 [email protected]
+	SPI
+	Wire
+
+lib_archive = false
+monitor_speed = 115200

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;
+}

src/Command.h

@@ -0,0 +1,17 @@
+#include <Arduino.h>
+// 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
+
+}; 

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   :  [email protected]
+ */
+
+#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; };