cbb/Motor_v2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix:修复任务调度不准确的问题，

Browse Source 
feat:优化电流采样速率
feat:优化PIDramp效率
feat:优化角度采样效率
fix:回滚CAN打包逻辑
feat:去除timer3
feat(FOC): 优化帕克逆变换与克拉克逆变换计算方式
将原有的三角函数计算替换为查表法，提高运算效率。同时添加了 sqrt(3)/2 的预计算常量，并优化了电压限制部分的计算逻辑，减少重复运算。
This commit is contained in:
cbb
2025-11-23 18:31:47 +08:00
parent 59f161bd73
commit 871e33e10f
14 changed files with 503 additions and 189 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.continue/mcpServers/new-mcp-server.yaml
+10
View File
@@ -0,0 +1,10 @@
name: New MCP server
version: 0.0.1
schema: v1
mcpServers:
- name: New MCP server
command: npx
args:
- -y
- <your-mcp-server>
env: {}
Robot_V2.csolution.yml
+1 -1
View File
@@ -1,6 +1,6 @@
# A solution is a collection of related projects that share same base configuration.
solution:
created-for: CMSIS-Toolbox@2.6.0
created-for: CMSIS-Toolbox@2.12.0
cdefault: # use CMSIS-Toolbox default Compiler options
# Compilers that can be selected
Robot_V2/Algorithm/DengFOC.cpp
+16 -12
View File
@@ -17,8 +17,10 @@ constexpr float _2_SQRT3 = 1.15470053838f;
constexpr float _3PI_2 = 4.71238898038f;
constexpr u8 PP = 11;
constexpr float DIR = 1.0;
constexpr float sqrt3 = 1.73205080757f; // sqrt(3)的预计算值
constexpr float sqrt3 = 1.73205080757f; // sqrt(3)的预计算值
constexpr float sqrt3dv2 = 0.86602540378f; // sqrt(3)/2的预计算值
constexpr float zero_electric_angle = 0.0f;
constexpr u16 LUT_SCALE = (LUT_SIZE / (2.0f * PI));
FOC_Controlroller FOC; // FOC控制初始化
extern sampler sampler;
@@ -63,24 +65,26 @@ inline void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud) {
float angle_el = _normalizeAngle(shaft_angle * PP);
// 帕克逆变换 (直接计算，避免函数调用开销)
const float sin_el = sin(angle_el);
const float cos_el = cos(angle_el);
Ualpha = -Uq * sin_el;
Ubeta = Uq * cos_el;
float sin_el, cos_el;
sin_el = sin_lut[(u16)(angle_el * LUT_SCALE) & LUT_MASK];
cos_el = cos_lut[(u16)(angle_el * LUT_SCALE) & LUT_MASK];
float Ualpha = Ud * cos_el - Uq * sin_el;
float Ubeta = Ud * sin_el + Uq * cos_el;
// 克拉克逆变换 (预计算常量值)
// 克拉克逆变换 (使用预计算常量优化)
Ua = Ualpha + v_half;
Ub = (sqrt3 * Ubeta - Ualpha) * 0.5f + v_half;
Uc = (-Ualpha - sqrt3 * Ubeta) * 0.5f + v_half;
Ub = sqrt3dv2 * Ubeta - 0.5f * Ualpha + v_half;
Uc = -0.5f * Ualpha - sqrt3dv2 * Ubeta + v_half;
// 计算占空比
// 计算占空比并限制范围
float dc_a = _constrain(Ua / v_safe, 0.0f, 1.0f);
float dc_b = _constrain(Ub / v_safe, 0.0f, 1.0f);
float dc_c = _constrain(Uc / v_safe, 0.0f, 1.0f);
// 设置PWM输出
dc_a = dc_a * limit_voltage / v_safe;
dc_b = dc_b * limit_voltage / v_safe;
dc_c = dc_c * limit_voltage / v_safe;
float voltage_exchange = limit_voltage / v_safe;
dc_a = dc_a * voltage_exchange;
dc_b = dc_b * voltage_exchange;
dc_c = dc_c * voltage_exchange;
Set_3Pwm(dc_a, dc_b, dc_c);
// std::printf("PWM:%f,%f,%f\n", dc_a, dc_b, dc_c);
}
Robot_V2/Algorithm/DengFOC.h
+350
View File
@@ -3,7 +3,9 @@
#define _DENGFOC_H
#include "pid.h"
#include <stdint.h>
class FOC_Controlroller {
public:
FOC_Controlroller();
@@ -57,4 +59,352 @@ void DFOC_Vbus(float power_supply);
inline float _electricalAngle();
float DFOC_M0_Angle(void);
constexpr u16 LUT_SIZE = 1024;
constexpr u16 LUT_MASK = (LUT_SIZE - 1);
__attribute__((aligned(4))) const float sin_lut[LUT_SIZE] = {
0.000000f, 0.006136f, 0.012272f, 0.018407f, 0.024541f, 0.030675f,
0.036807f, 0.042938f, 0.049068f, 0.055195f, 0.061321f, 0.067444f,
0.073565f, 0.079682f, 0.085797f, 0.091909f, 0.098017f, 0.104122f,
0.110222f, 0.116319f, 0.122411f, 0.128498f, 0.134581f, 0.140658f,
0.146730f, 0.152797f, 0.158858f, 0.164913f, 0.170962f, 0.177004f,
0.183040f, 0.189069f, 0.195090f, 0.201105f, 0.207111f, 0.213110f,
0.219101f, 0.225084f, 0.231058f, 0.237024f, 0.242980f, 0.248928f,
0.254866f, 0.260794f, 0.266713f, 0.272621f, 0.278520f, 0.284408f,
0.290285f, 0.296151f, 0.302006f, 0.307850f, 0.313682f, 0.319502f,
0.325310f, 0.331106f, 0.336890f, 0.342661f, 0.348419f, 0.354164f,
0.359895f, 0.365613f, 0.371317f, 0.377007f, 0.382683f, 0.388345f,
0.393992f, 0.399624f, 0.405241f, 0.410843f, 0.416430f, 0.422000f,
0.427555f, 0.433094f, 0.438616f, 0.444122f, 0.449611f, 0.455084f,
0.460539f, 0.465976f, 0.471397f, 0.476799f, 0.482184f, 0.487550f,
0.492898f, 0.498228f, 0.503538f, 0.508830f, 0.514103f, 0.519356f,
0.524590f, 0.529804f, 0.534998f, 0.540171f, 0.545325f, 0.550458f,
0.555570f, 0.560662f, 0.565732f, 0.570781f, 0.575808f, 0.580814f,
0.585798f, 0.590760f, 0.595699f, 0.600616f, 0.605511f, 0.610383f,
0.615232f, 0.620057f, 0.624859f, 0.629638f, 0.634393f, 0.639124f,
0.643832f, 0.648514f, 0.653173f, 0.657807f, 0.662416f, 0.667000f,
0.671559f, 0.676093f, 0.680601f, 0.685084f, 0.689541f, 0.693971f,
0.698376f, 0.702755f, 0.707107f, 0.711432f, 0.715731f, 0.720003f,
0.724247f, 0.728464f, 0.732654f, 0.736817f, 0.740951f, 0.745058f,
0.749136f, 0.753187f, 0.757209f, 0.761202f, 0.765167f, 0.769103f,
0.773010f, 0.776888f, 0.780737f, 0.784557f, 0.788346f, 0.792107f,
0.795837f, 0.799537f, 0.803208f, 0.806848f, 0.810457f, 0.814036f,
0.817585f, 0.821103f, 0.824589f, 0.828045f, 0.831470f, 0.834863f,
0.838225f, 0.841555f, 0.844854f, 0.848120f, 0.851355f, 0.854558f,
0.857729f, 0.860867f, 0.863973f, 0.867046f, 0.870087f, 0.873095f,
0.876070f, 0.879012f, 0.881921f, 0.884797f, 0.887640f, 0.890449f,
0.893224f, 0.895966f, 0.898674f, 0.901349f, 0.903989f, 0.906596f,
0.909168f, 0.911706f, 0.914210f, 0.916679f, 0.919114f, 0.921514f,
0.923880f, 0.926210f, 0.928506f, 0.930767f, 0.932993f, 0.935184f,
0.937339f, 0.939459f, 0.941544f, 0.943593f, 0.945607f, 0.947586f,
0.949528f, 0.951435f, 0.953306f, 0.955141f, 0.956940f, 0.958703f,
0.960431f, 0.962121f, 0.963776f, 0.965394f, 0.966976f, 0.968522f,
0.970031f, 0.971504f, 0.972940f, 0.974339f, 0.975702f, 0.977028f,
0.978317f, 0.979570f, 0.980785f, 0.981964f, 0.983105f, 0.984210f,
0.985278f, 0.986308f, 0.987301f, 0.988258f, 0.989177f, 0.990058f,
0.990903f, 0.991710f, 0.992480f, 0.993212f, 0.993907f, 0.994565f,
0.995185f, 0.995767f, 0.996313f, 0.996820f, 0.997290f, 0.997723f,
0.998118f, 0.998476f, 0.998795f, 0.999078f, 0.999322f, 0.999529f,
0.999699f, 0.999831f, 0.999925f, 0.999981f, 1.000000f, 0.999981f,
0.999925f, 0.999831f, 0.999699f, 0.999529f, 0.999322f, 0.999078f,
0.998795f, 0.998476f, 0.998118f, 0.997723f, 0.997290f, 0.996820f,
0.996313f, 0.995767f, 0.995185f, 0.994565f, 0.993907f, 0.993212f,
0.992480f, 0.991710f, 0.990903f, 0.990058f, 0.989177f, 0.988258f,
0.987301f, 0.986308f, 0.985278f, 0.984210f, 0.983105f, 0.981964f,
0.980785f, 0.979570f, 0.978317f, 0.977028f, 0.975702f, 0.974339f,
0.972940f, 0.971504f, 0.970031f, 0.968522f, 0.966976f, 0.965394f,
0.963776f, 0.962121f, 0.960431f, 0.958703f, 0.956940f, 0.955141f,
0.953306f, 0.951435f, 0.949528f, 0.947586f, 0.945607f, 0.943593f,
0.941544f, 0.939459f, 0.937339f, 0.935184f, 0.932993f, 0.930767f,
0.928506f, 0.926210f, 0.923880f, 0.921514f, 0.919114f, 0.916679f,
0.914210f, 0.911706f, 0.909168f, 0.906596f, 0.903989f, 0.901349f,
0.898674f, 0.895966f, 0.893224f, 0.890449f, 0.887640f, 0.884797f,
0.881921f, 0.879012f, 0.876070f, 0.873095f, 0.870087f, 0.867046f,
0.863973f, 0.860867f, 0.857729f, 0.854558f, 0.851355f, 0.848120f,
0.844854f, 0.841555f, 0.838225f, 0.834863f, 0.831470f, 0.828045f,
0.824589f, 0.821103f, 0.817585f, 0.814036f, 0.810457f, 0.806848f,
0.803208f, 0.799537f, 0.795837f, 0.792107f, 0.788346f, 0.784557f,
0.780737f, 0.776888f, 0.773010f, 0.769103f, 0.765167f, 0.761202f,
0.757209f, 0.753187f, 0.749136f, 0.745058f, 0.740951f, 0.736817f,
0.732654f, 0.728464f, 0.724247f, 0.720003f, 0.715731f, 0.711432f,
0.707107f, 0.702755f, 0.698376f, 0.693971f, 0.689541f, 0.685084f,
0.680601f, 0.676093f, 0.671559f, 0.667000f, 0.662416f, 0.657807f,
0.653173f, 0.648514f, 0.643832f, 0.639124f, 0.634393f, 0.629638f,
0.624859f, 0.620057f, 0.615232f, 0.610383f, 0.605511f, 0.600616f,
0.595699f, 0.590760f, 0.585798f, 0.580814f, 0.575808f, 0.570781f,
0.565732f, 0.560662f, 0.555570f, 0.550458f, 0.545325f, 0.540171f,
0.534998f, 0.529804f, 0.524590f, 0.519356f, 0.514103f, 0.508830f,
0.503538f, 0.498228f, 0.492898f, 0.487550f, 0.482184f, 0.476799f,
0.471397f, 0.465976f, 0.460539f, 0.455084f, 0.449611f, 0.444122f,
0.438616f, 0.433094f, 0.427555f, 0.422000f, 0.416430f, 0.410843f,
0.405241f, 0.399624f, 0.393992f, 0.388345f, 0.382683f, 0.377007f,
0.371317f, 0.365613f, 0.359895f, 0.354164f, 0.348419f, 0.342661f,
0.336890f, 0.331106f, 0.325310f, 0.319502f, 0.313682f, 0.307850f,
0.302006f, 0.296151f, 0.290285f, 0.284408f, 0.278520f, 0.272621f,
0.266713f, 0.260794f, 0.254866f, 0.248928f, 0.242980f, 0.237024f,
0.231058f, 0.225084f, 0.219101f, 0.213110f, 0.207111f, 0.201105f,
0.195090f, 0.189069f, 0.183040f, 0.177004f, 0.170962f, 0.164913f,
0.158858f, 0.152797f, 0.146730f, 0.140658f, 0.134581f, 0.128498f,
0.122411f, 0.116319f, 0.110222f, 0.104122f, 0.098017f, 0.091909f,
0.085797f, 0.079682f, 0.073565f, 0.067444f, 0.061321f, 0.055195f,
0.049068f, 0.042938f, 0.036807f, 0.030675f, 0.024541f, 0.018407f,
0.012272f, 0.006136f, 0.000000f, -0.006136f, -0.012272f, -0.018407f,
-0.024541f, -0.030675f, -0.036807f, -0.042938f, -0.049068f, -0.055195f,
-0.061321f, -0.067444f, -0.073565f, -0.079682f, -0.085797f, -0.091909f,
-0.098017f, -0.104122f, -0.110222f, -0.116319f, -0.122411f, -0.128498f,
-0.134581f, -0.140658f, -0.146730f, -0.152797f, -0.158858f, -0.164913f,
-0.170962f, -0.177004f, -0.183040f, -0.189069f, -0.195090f, -0.201105f,
-0.207111f, -0.213110f, -0.219101f, -0.225084f, -0.231058f, -0.237024f,
-0.242980f, -0.248928f, -0.254866f, -0.260794f, -0.266713f, -0.272621f,
-0.278520f, -0.284408f, -0.290285f, -0.296151f, -0.302006f, -0.307850f,
-0.313682f, -0.319502f, -0.325310f, -0.331106f, -0.336890f, -0.342661f,
-0.348419f, -0.354164f, -0.359895f, -0.365613f, -0.371317f, -0.377007f,
-0.382683f, -0.388345f, -0.393992f, -0.399624f, -0.405241f, -0.410843f,
-0.416430f, -0.422000f, -0.427555f, -0.433094f, -0.438616f, -0.444122f,
-0.449611f, -0.455084f, -0.460539f, -0.465976f, -0.471397f, -0.476799f,
-0.482184f, -0.487550f, -0.492898f, -0.498228f, -0.503538f, -0.508830f,
-0.514103f, -0.519356f, -0.524590f, -0.529804f, -0.534998f, -0.540171f,
-0.545325f, -0.550458f, -0.555570f, -0.560662f, -0.565732f, -0.570781f,
-0.575808f, -0.580814f, -0.585798f, -0.590760f, -0.595699f, -0.600616f,
-0.605511f, -0.610383f, -0.615232f, -0.620057f, -0.624859f, -0.629638f,
-0.634393f, -0.639124f, -0.643832f, -0.648514f, -0.653173f, -0.657807f,
-0.662416f, -0.667000f, -0.671559f, -0.676093f, -0.680601f, -0.685084f,
-0.689541f, -0.693971f, -0.698376f, -0.702755f, -0.707107f, -0.711432f,
-0.715731f, -0.720003f, -0.724247f, -0.728464f, -0.732654f, -0.736817f,
-0.740951f, -0.745058f, -0.749136f, -0.753187f, -0.757209f, -0.761202f,
-0.765167f, -0.769103f, -0.773010f, -0.776888f, -0.780737f, -0.784557f,
-0.788346f, -0.792107f, -0.795837f, -0.799537f, -0.803208f, -0.806848f,
-0.810457f, -0.814036f, -0.817585f, -0.821103f, -0.824589f, -0.828045f,
-0.831470f, -0.834863f, -0.838225f, -0.841555f, -0.844854f, -0.848120f,
-0.851355f, -0.854558f, -0.857729f, -0.860867f, -0.863973f, -0.867046f,
-0.870087f, -0.873095f, -0.876070f, -0.879012f, -0.881921f, -0.884797f,
-0.887640f, -0.890449f, -0.893224f, -0.895966f, -0.898674f, -0.901349f,
-0.903989f, -0.906596f, -0.909168f, -0.911706f, -0.914210f, -0.916679f,
-0.919114f, -0.921514f, -0.923880f, -0.926210f, -0.928506f, -0.930767f,
-0.932993f, -0.935184f, -0.937339f, -0.939459f, -0.941544f, -0.943593f,
-0.945607f, -0.947586f, -0.949528f, -0.951435f, -0.953306f, -0.955141f,
-0.956940f, -0.958703f, -0.960431f, -0.962121f, -0.963776f, -0.965394f,
-0.966976f, -0.968522f, -0.970031f, -0.971504f, -0.972940f, -0.974339f,
-0.975702f, -0.977028f, -0.978317f, -0.979570f, -0.980785f, -0.981964f,
-0.983105f, -0.984210f, -0.985278f, -0.986308f, -0.987301f, -0.988258f,
-0.989177f, -0.990058f, -0.990903f, -0.991710f, -0.992480f, -0.993212f,
-0.993907f, -0.994565f, -0.995185f, -0.995767f, -0.996313f, -0.996820f,
-0.997290f, -0.997723f, -0.998118f, -0.998476f, -0.998795f, -0.999078f,
-0.999322f, -0.999529f, -0.999699f, -0.999831f, -0.999925f, -0.999981f,
-1.000000f, -0.999981f, -0.999925f, -0.999831f, -0.999699f, -0.999529f,
-0.999322f, -0.999078f, -0.998795f, -0.998476f, -0.998118f, -0.997723f,
-0.997290f, -0.996820f, -0.996313f, -0.995767f, -0.995185f, -0.994565f,
-0.993907f, -0.993212f, -0.992480f, -0.991710f, -0.990903f, -0.990058f,
-0.989177f, -0.988258f, -0.987301f, -0.986308f, -0.985278f, -0.984210f,
-0.983105f, -0.981964f, -0.980785f, -0.979570f, -0.978317f, -0.977028f,
-0.975702f, -0.974339f, -0.972940f, -0.971504f, -0.970031f, -0.968522f,
-0.966976f, -0.965394f, -0.963776f, -0.962121f, -0.960431f, -0.958703f,
-0.956940f, -0.955141f, -0.953306f, -0.951435f, -0.949528f, -0.947586f,
-0.945607f, -0.943593f, -0.941544f, -0.939459f, -0.937339f, -0.935184f,
-0.932993f, -0.930767f, -0.928506f, -0.926210f, -0.923880f, -0.921514f,
-0.919114f, -0.916679f, -0.914210f, -0.911706f, -0.909168f, -0.906596f,
-0.903989f, -0.901349f, -0.898674f, -0.895966f, -0.893224f, -0.890449f,
-0.887640f, -0.884797f, -0.881921f, -0.879012f, -0.876070f, -0.873095f,
-0.870087f, -0.867046f, -0.863973f, -0.860867f, -0.857729f, -0.854558f,
-0.851355f, -0.848120f, -0.844854f, -0.841555f, -0.838225f, -0.834863f,
-0.831470f, -0.828045f, -0.824589f, -0.821103f, -0.817585f, -0.814036f,
-0.810457f, -0.806848f, -0.803208f, -0.799537f, -0.795837f, -0.792107f,
-0.788346f, -0.784557f, -0.780737f, -0.776888f, -0.773010f, -0.769103f,
-0.765167f, -0.761202f, -0.757209f, -0.753187f, -0.749136f, -0.745058f,
-0.740951f, -0.736817f, -0.732654f, -0.728464f, -0.724247f, -0.720003f,
-0.715731f, -0.711432f, -0.707107f, -0.702755f, -0.698376f, -0.693971f,
-0.689541f, -0.685084f, -0.680601f, -0.676093f, -0.671559f, -0.667000f,
-0.662416f, -0.657807f, -0.653173f, -0.648514f, -0.643832f, -0.639124f,
-0.634393f, -0.629638f, -0.624859f, -0.620057f, -0.615232f, -0.610383f,
-0.605511f, -0.600616f, -0.595699f, -0.590760f, -0.585798f, -0.580814f,
-0.575808f, -0.570781f, -0.565732f, -0.560662f, -0.555570f, -0.550458f,
-0.545325f, -0.540171f, -0.534998f, -0.529804f, -0.524590f, -0.519356f,
-0.514103f, -0.508830f, -0.503538f, -0.498228f, -0.492898f, -0.487550f,
-0.482184f, -0.476799f, -0.471397f, -0.465976f, -0.460539f, -0.455084f,
-0.449611f, -0.444122f, -0.438616f, -0.433094f, -0.427555f, -0.422000f,
-0.416430f, -0.410843f, -0.405241f, -0.399624f, -0.393992f, -0.388345f,
-0.382683f, -0.377007f, -0.371317f, -0.365613f, -0.359895f, -0.354164f,
-0.348419f, -0.342661f, -0.336890f, -0.331106f, -0.325310f, -0.319502f,
-0.313682f, -0.307850f, -0.302006f, -0.296151f, -0.290285f, -0.284408f,
-0.278520f, -0.272621f, -0.266713f, -0.260794f, -0.254866f, -0.248928f,
-0.242980f, -0.237024f, -0.231058f, -0.225084f, -0.219101f, -0.213110f,
-0.207111f, -0.201105f, -0.195090f, -0.189069f, -0.183040f, -0.177004f,
-0.170962f, -0.164913f, -0.158858f, -0.152797f, -0.146730f, -0.140658f,
-0.134581f, -0.128498f, -0.122411f, -0.116319f, -0.110222f, -0.104122f,
-0.098017f, -0.091909f, -0.085797f, -0.079682f, -0.073565f, -0.067444f,
-0.061321f, -0.055195f, -0.049068f, -0.042938f, -0.036807f, -0.030675f,
-0.024541f, -0.018407f, -0.012272f, -0.006136f};
__attribute__((aligned(4))) const float cos_lut[LUT_SIZE] = {
1.000000f, 0.999981f, 0.999925f, 0.999831f, 0.999699f, 0.999529f,
0.999322f, 0.999078f, 0.998795f, 0.998476f, 0.998118f, 0.997723f,
0.997290f, 0.996820f, 0.996313f, 0.995767f, 0.995185f, 0.994565f,
0.993907f, 0.993212f, 0.992480f, 0.991710f, 0.990903f, 0.990058f,
0.989177f, 0.988258f, 0.987301f, 0.986308f, 0.985278f, 0.984210f,
0.983105f, 0.981964f, 0.980785f, 0.979570f, 0.978317f, 0.977028f,
0.975702f, 0.974339f, 0.972940f, 0.971504f, 0.970031f, 0.968522f,
0.966976f, 0.965394f, 0.963776f, 0.962121f, 0.960431f, 0.958703f,
0.956940f, 0.955141f, 0.953306f, 0.951435f, 0.949528f, 0.947586f,
0.945607f, 0.943593f, 0.941544f, 0.939459f, 0.937339f, 0.935184f,
0.932993f, 0.930767f, 0.928506f, 0.926210f, 0.923880f, 0.921514f,
0.919114f, 0.916679f, 0.914210f, 0.911706f, 0.909168f, 0.906596f,
0.903989f, 0.901349f, 0.898674f, 0.895966f, 0.893224f, 0.890449f,
0.887640f, 0.884797f, 0.881921f, 0.879012f, 0.876070f, 0.873095f,
0.870087f, 0.867046f, 0.863973f, 0.860867f, 0.857729f, 0.854558f,
0.851355f, 0.848120f, 0.844854f, 0.841555f, 0.838225f, 0.834863f,
0.831470f, 0.828045f, 0.824589f, 0.821103f, 0.817585f, 0.814036f,
0.810457f, 0.806848f, 0.803208f, 0.799537f, 0.795837f, 0.792107f,
0.788346f, 0.784557f, 0.780737f, 0.776888f, 0.773010f, 0.769103f,
0.765167f, 0.761202f, 0.757209f, 0.753187f, 0.749136f, 0.745058f,
0.740951f, 0.736817f, 0.732654f, 0.728464f, 0.724247f, 0.720003f,
0.715731f, 0.711432f, 0.707107f, 0.702755f, 0.698376f, 0.693971f,
0.689541f, 0.685084f, 0.680601f, 0.676093f, 0.671559f, 0.667000f,
0.662416f, 0.657807f, 0.653173f, 0.648514f, 0.643832f, 0.639124f,
0.634393f, 0.629638f, 0.624859f, 0.620057f, 0.615232f, 0.610383f,
0.605511f, 0.600616f, 0.595699f, 0.590760f, 0.585798f, 0.580814f,
0.575808f, 0.570781f, 0.565732f, 0.560662f, 0.555570f, 0.550458f,
0.545325f, 0.540171f, 0.534998f, 0.529804f, 0.524590f, 0.519356f,
0.514103f, 0.508830f, 0.503538f, 0.498228f, 0.492898f, 0.487550f,
0.482184f, 0.476799f, 0.471397f, 0.465976f, 0.460539f, 0.455084f,
0.449611f, 0.444122f, 0.438616f, 0.433094f, 0.427555f, 0.422000f,
0.416430f, 0.410843f, 0.405241f, 0.399624f, 0.393992f, 0.388345f,
0.382683f, 0.377007f, 0.371317f, 0.365613f, 0.359895f, 0.354164f,
0.348419f, 0.342661f, 0.336890f, 0.331106f, 0.325310f, 0.319502f,
0.313682f, 0.307850f, 0.302006f, 0.296151f, 0.290285f, 0.284408f,
0.278520f, 0.272621f, 0.266713f, 0.260794f, 0.254866f, 0.248928f,
0.242980f, 0.237024f, 0.231058f, 0.225084f, 0.219101f, 0.213110f,
0.207111f, 0.201105f, 0.195090f, 0.189069f, 0.183040f, 0.177004f,
0.170962f, 0.164913f, 0.158858f, 0.152797f, 0.146730f, 0.140658f,
0.134581f, 0.128498f, 0.122411f, 0.116319f, 0.110222f, 0.104122f,
0.098017f, 0.091909f, 0.085797f, 0.079682f, 0.073565f, 0.067444f,
0.061321f, 0.055195f, 0.049068f, 0.042938f, 0.036807f, 0.030675f,
0.024541f, 0.018407f, 0.012272f, 0.006136f, 0.000000f, -0.006136f,
-0.012272f, -0.018407f, -0.024541f, -0.030675f, -0.036807f, -0.042938f,
-0.049068f, -0.055195f, -0.061321f, -0.067444f, -0.073565f, -0.079682f,
-0.085797f, -0.091909f, -0.098017f, -0.104122f, -0.110222f, -0.116319f,
-0.122411f, -0.128498f, -0.134581f, -0.140658f, -0.146730f, -0.152797f,
-0.158858f, -0.164913f, -0.170962f, -0.177004f, -0.183040f, -0.189069f,
-0.195090f, -0.201105f, -0.207111f, -0.213110f, -0.219101f, -0.225084f,
-0.231058f, -0.237024f, -0.242980f, -0.248928f, -0.254866f, -0.260794f,
-0.266713f, -0.272621f, -0.278520f, -0.284408f, -0.290285f, -0.296151f,
-0.302006f, -0.307850f, -0.313682f, -0.319502f, -0.325310f, -0.331106f,
-0.336890f, -0.342661f, -0.348419f, -0.354164f, -0.359895f, -0.365613f,
-0.371317f, -0.377007f, -0.382683f, -0.388345f, -0.393992f, -0.399624f,
-0.405241f, -0.410843f, -0.416430f, -0.422000f, -0.427555f, -0.433094f,
-0.438616f, -0.444122f, -0.449611f, -0.455084f, -0.460539f, -0.465976f,
-0.471397f, -0.476799f, -0.482184f, -0.487550f, -0.492898f, -0.498228f,
-0.503538f, -0.508830f, -0.514103f, -0.519356f, -0.524590f, -0.529804f,
-0.534998f, -0.540171f, -0.545325f, -0.550458f, -0.555570f, -0.560662f,
-0.565732f, -0.570781f, -0.575808f, -0.580814f, -0.585798f, -0.590760f,
-0.595699f, -0.600616f, -0.605511f, -0.610383f, -0.615232f, -0.620057f,
-0.624859f, -0.629638f, -0.634393f, -0.639124f, -0.643832f, -0.648514f,
-0.653173f, -0.657807f, -0.662416f, -0.667000f, -0.671559f, -0.676093f,
-0.680601f, -0.685084f, -0.689541f, -0.693971f, -0.698376f, -0.702755f,
-0.707107f, -0.711432f, -0.715731f, -0.720003f, -0.724247f, -0.728464f,
-0.732654f, -0.736817f, -0.740951f, -0.745058f, -0.749136f, -0.753187f,
-0.757209f, -0.761202f, -0.765167f, -0.769103f, -0.773010f, -0.776888f,
-0.780737f, -0.784557f, -0.788346f, -0.792107f, -0.795837f, -0.799537f,
-0.803208f, -0.806848f, -0.810457f, -0.814036f, -0.817585f, -0.821103f,
-0.824589f, -0.828045f, -0.831470f, -0.834863f, -0.838225f, -0.841555f,
-0.844854f, -0.848120f, -0.851355f, -0.854558f, -0.857729f, -0.860867f,
-0.863973f, -0.867046f, -0.870087f, -0.873095f, -0.876070f, -0.879012f,
-0.881921f, -0.884797f, -0.887640f, -0.890449f, -0.893224f, -0.895966f,
-0.898674f, -0.901349f, -0.903989f, -0.906596f, -0.909168f, -0.911706f,
-0.914210f, -0.916679f, -0.919114f, -0.921514f, -0.923880f, -0.926210f,
-0.928506f, -0.930767f, -0.932993f, -0.935184f, -0.937339f, -0.939459f,
-0.941544f, -0.943593f, -0.945607f, -0.947586f, -0.949528f, -0.951435f,
-0.953306f, -0.955141f, -0.956940f, -0.958703f, -0.960431f, -0.962121f,
-0.963776f, -0.965394f, -0.966976f, -0.968522f, -0.970031f, -0.971504f,
-0.972940f, -0.974339f, -0.975702f, -0.977028f, -0.978317f, -0.979570f,
-0.980785f, -0.981964f, -0.983105f, -0.984210f, -0.985278f, -0.986308f,
-0.987301f, -0.988258f, -0.989177f, -0.990058f, -0.990903f, -0.991710f,
-0.992480f, -0.993212f, -0.993907f, -0.994565f, -0.995185f, -0.995767f,
-0.996313f, -0.996820f, -0.997290f, -0.997723f, -0.998118f, -0.998476f,
-0.998795f, -0.999078f, -0.999322f, -0.999529f, -0.999699f, -0.999831f,
-0.999925f, -0.999981f, -1.000000f, -0.999981f, -0.999925f, -0.999831f,
-0.999699f, -0.999529f, -0.999322f, -0.999078f, -0.998795f, -0.998476f,
-0.998118f, -0.997723f, -0.997290f, -0.996820f, -0.996313f, -0.995767f,
-0.995185f, -0.994565f, -0.993907f, -0.993212f, -0.992480f, -0.991710f,
-0.990903f, -0.990058f, -0.989177f, -0.988258f, -0.987301f, -0.986308f,
-0.985278f, -0.984210f, -0.983105f, -0.981964f, -0.980785f, -0.979570f,
-0.978317f, -0.977028f, -0.975702f, -0.974339f, -0.972940f, -0.971504f,
-0.970031f, -0.968522f, -0.966976f, -0.965394f, -0.963776f, -0.962121f,
-0.960431f, -0.958703f, -0.956940f, -0.955141f, -0.953306f, -0.951435f,
-0.949528f, -0.947586f, -0.945607f, -0.943593f, -0.941544f, -0.939459f,
-0.937339f, -0.935184f, -0.932993f, -0.930767f, -0.928506f, -0.926210f,
-0.923880f, -0.921514f, -0.919114f, -0.916679f, -0.914210f, -0.911706f,
-0.909168f, -0.906596f, -0.903989f, -0.901349f, -0.898674f, -0.895966f,
-0.893224f, -0.890449f, -0.887640f, -0.884797f, -0.881921f, -0.879012f,
-0.876070f, -0.873095f, -0.870087f, -0.867046f, -0.863973f, -0.860867f,
-0.857729f, -0.854558f, -0.851355f, -0.848120f, -0.844854f, -0.841555f,
-0.838225f, -0.834863f, -0.831470f, -0.828045f, -0.824589f, -0.821103f,
-0.817585f, -0.814036f, -0.810457f, -0.806848f, -0.803208f, -0.799537f,
-0.795837f, -0.792107f, -0.788346f, -0.784557f, -0.780737f, -0.776888f,
-0.773010f, -0.769103f, -0.765167f, -0.761202f, -0.757209f, -0.753187f,
-0.749136f, -0.745058f, -0.740951f, -0.736817f, -0.732654f, -0.728464f,
-0.724247f, -0.720003f, -0.715731f, -0.711432f, -0.707107f, -0.702755f,
-0.698376f, -0.693971f, -0.689541f, -0.685084f, -0.680601f, -0.676093f,
-0.671559f, -0.667000f, -0.662416f, -0.657807f, -0.653173f, -0.648514f,
-0.643832f, -0.639124f, -0.634393f, -0.629638f, -0.624859f, -0.620057f,
-0.615232f, -0.610383f, -0.605511f, -0.600616f, -0.595699f, -0.590760f,
-0.585798f, -0.580814f, -0.575808f, -0.570781f, -0.565732f, -0.560662f,
-0.555570f, -0.550458f, -0.545325f, -0.540171f, -0.534998f, -0.529804f,
-0.524590f, -0.519356f, -0.514103f, -0.508830f, -0.503538f, -0.498228f,
-0.492898f, -0.487550f, -0.482184f, -0.476799f, -0.471397f, -0.465976f,
-0.460539f, -0.455084f, -0.449611f, -0.444122f, -0.438616f, -0.433094f,
-0.427555f, -0.422000f, -0.416430f, -0.410843f, -0.405241f, -0.399624f,
-0.393992f, -0.388345f, -0.382683f, -0.377007f, -0.371317f, -0.365613f,
-0.359895f, -0.354164f, -0.348419f, -0.342661f, -0.336890f, -0.331106f,
-0.325310f, -0.319502f, -0.313682f, -0.307850f, -0.302006f, -0.296151f,
-0.290285f, -0.284408f, -0.278520f, -0.272621f, -0.266713f, -0.260794f,
-0.254866f, -0.248928f, -0.242980f, -0.237024f, -0.231058f, -0.225084f,
-0.219101f, -0.213110f, -0.207111f, -0.201105f, -0.195090f, -0.189069f,
-0.183040f, -0.177004f, -0.170962f, -0.164913f, -0.158858f, -0.152797f,
-0.146730f, -0.140658f, -0.134581f, -0.128498f, -0.122411f, -0.116319f,
-0.110222f, -0.104122f, -0.098017f, -0.091909f, -0.085797f, -0.079682f,
-0.073565f, -0.067444f, -0.061321f, -0.055195f, -0.049068f, -0.042938f,
-0.036807f, -0.030675f, -0.024541f, -0.018407f, -0.012272f, -0.006136f,
-0.000000f, 0.006136f, 0.012272f, 0.018407f, 0.024541f, 0.030675f,
0.036807f, 0.042938f, 0.049068f, 0.055195f, 0.061321f, 0.067444f,
0.073565f, 0.079682f, 0.085797f, 0.091909f, 0.098017f, 0.104122f,
0.110222f, 0.116319f, 0.122411f, 0.128498f, 0.134581f, 0.140658f,
0.146730f, 0.152797f, 0.158858f, 0.164913f, 0.170962f, 0.177004f,
0.183040f, 0.189069f, 0.195090f, 0.201105f, 0.207111f, 0.213110f,
0.219101f, 0.225084f, 0.231058f, 0.237024f, 0.242980f, 0.248928f,
0.254866f, 0.260794f, 0.266713f, 0.272621f, 0.278520f, 0.284408f,
0.290285f, 0.296151f, 0.302006f, 0.307850f, 0.313682f, 0.319502f,
0.325310f, 0.331106f, 0.336890f, 0.342661f, 0.348419f, 0.354164f,
0.359895f, 0.365613f, 0.371317f, 0.377007f, 0.382683f, 0.388345f,
0.393992f, 0.399624f, 0.405241f, 0.410843f, 0.416430f, 0.422000f,
0.427555f, 0.433094f, 0.438616f, 0.444122f, 0.449611f, 0.455084f,
0.460539f, 0.465976f, 0.471397f, 0.476799f, 0.482184f, 0.487550f,
0.492898f, 0.498228f, 0.503538f, 0.508830f, 0.514103f, 0.519356f,
0.524590f, 0.529804f, 0.534998f, 0.540171f, 0.545325f, 0.550458f,
0.555570f, 0.560662f, 0.565732f, 0.570781f, 0.575808f, 0.580814f,
0.585798f, 0.590760f, 0.595699f, 0.600616f, 0.605511f, 0.610383f,
0.615232f, 0.620057f, 0.624859f, 0.629638f, 0.634393f, 0.639124f,
0.643832f, 0.648514f, 0.653173f, 0.657807f, 0.662416f, 0.667000f,
0.671559f, 0.676093f, 0.680601f, 0.685084f, 0.689541f, 0.693971f,
0.698376f, 0.702755f, 0.707107f, 0.711432f, 0.715731f, 0.720003f,
0.724247f, 0.728464f, 0.732654f, 0.736817f, 0.740951f, 0.745058f,
0.749136f, 0.753187f, 0.757209f, 0.761202f, 0.765167f, 0.769103f,
0.773010f, 0.776888f, 0.780737f, 0.784557f, 0.788346f, 0.792107f,
0.795837f, 0.799537f, 0.803208f, 0.806848f, 0.810457f, 0.814036f,
0.817585f, 0.821103f, 0.824589f, 0.828045f, 0.831470f, 0.834863f,
0.838225f, 0.841555f, 0.844854f, 0.848120f, 0.851355f, 0.854558f,
0.857729f, 0.860867f, 0.863973f, 0.867046f, 0.870087f, 0.873095f,
0.876070f, 0.879012f, 0.881921f, 0.884797f, 0.887640f, 0.890449f,
0.893224f, 0.895966f, 0.898674f, 0.901349f, 0.903989f, 0.906596f,
0.909168f, 0.911706f, 0.914210f, 0.916679f, 0.919114f, 0.921514f,
0.923880f, 0.926210f, 0.928506f, 0.930767f, 0.932993f, 0.935184f,
0.937339f, 0.939459f, 0.941544f, 0.943593f, 0.945607f, 0.947586f,
0.949528f, 0.951435f, 0.953306f, 0.955141f, 0.956940f, 0.958703f,
0.960431f, 0.962121f, 0.963776f, 0.965394f, 0.966976f, 0.968522f,
0.970031f, 0.971504f, 0.972940f, 0.974339f, 0.975702f, 0.977028f,
0.978317f, 0.979570f, 0.980785f, 0.981964f, 0.983105f, 0.984210f,
0.985278f, 0.986308f, 0.987301f, 0.988258f, 0.989177f, 0.990058f,
0.990903f, 0.991710f, 0.992480f, 0.993212f, 0.993907f, 0.994565f,
0.995185f, 0.995767f, 0.996313f, 0.996820f, 0.997290f, 0.997723f,
0.998118f, 0.998476f, 0.998795f, 0.999078f, 0.999322f, 0.999529f,
0.999699f, 0.999831f, 0.999925f, 0.999981f};
#endif
Robot_V2/Algorithm/pid.cpp
+6 -8
View File
@@ -2,6 +2,7 @@
#define _constrain(amt, low, high) \
((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt)))
#include "Can.h"
#include "math.h"
extern CAN_transmiter CAN0_transmiter;
constexpr float TIME_SLICE = 1e-4;
@@ -31,14 +32,11 @@ float PIDController::operator()(float error) {
output = _constrain(output, -limit, limit);
// 输出斜坡限制
if (output_ramp > 0) {
// 对PID的变化速率进行限制
float output_rate = (output - output_prev) / TIME_SLICE;
if (output_rate > output_ramp)
output = output_prev + output_ramp * TIME_SLICE;
else if (output_rate < -output_ramp)
output = output_prev - output_ramp * TIME_SLICE;
}
// 简化的斜坡限制（单行逻辑）
if (output_ramp > 0) {
float max_delta = output_ramp * TIME_SLICE;
output = output_prev + fmaxf(-max_delta, fminf(max_delta, output - output_prev));
}
// 保存值（为了下一次循环）
integral_prev = integral;
output_prev = output;
Robot_V2/device/ADC.cpp
+18 -14
View File
@@ -1,6 +1,6 @@
#include "adc.h"
#include "Filter.h"
#include "Can.h"
#include "Filter.h"
#include "math.h"
#include <cstring>
#include <gd32c10x_dma.h>
@@ -20,7 +20,7 @@ constexpr float MIN_CURRENT = -40.0f;
constexpr float MAX_VCC = 65.0f;
constexpr float MIN_VCC = -0.1f;
extern CAN_transmiter CAN0_transmiter;
sampler sampler; // 采样器初始化
sampler sampler; // 采样器初始化
u16 adc_value[6] = {0}; // 显式初始化数组元素为0
/*!
@@ -139,7 +139,7 @@ sampler::sampler() {
float sampler::get_mcu_temp() {
tempora_sample = medium_filter(mcu_temp_samples, FILTER_SIZE + 1);
// 检查温度读数是否在有效范围内
if (tempora_sample < MIN_TEMP || tempora_sample > MAX_TEMP) {
if (tempora_sample < MIN_TEMP or tempora_sample > MAX_TEMP) {
return ERROR_ADC_VAL;
}
return tempora_sample;
@@ -186,7 +186,7 @@ float sampler::get_current(char type) {
return ERROR_ADC_VAL;
}
}
if (tempora_sample < MIN_CURRENT || tempora_sample > MAX_CURRENT) {
if (tempora_sample < MIN_CURRENT or tempora_sample > MAX_CURRENT) {
return ERROR_ADC_VAL;
}
return tempora_sample;
@@ -202,7 +202,7 @@ float sampler::get_current(char type) {
float sampler::get_vbus_voltage() {
tempora_sample = medium_filter(vbus_samples, FILTER_SIZE + 1);
// 检查电压读数是否在有效范围内
if (vbus_samples[FILTER_SIZE / 2] > MAX_VCC ||
if (vbus_samples[FILTER_SIZE / 2] > MAX_VCC or
vbus_samples[FILTER_SIZE / 2] < MIN_VCC) {
return ERROR_ADC_VAL;
}
@@ -237,16 +237,20 @@ void sampler::update_drv_temp() {
* @brief 更新相电流
*/
void sampler::update_current() {
memmove(currU_samples, currU_samples + 1, FILTER_SIZE * sizeof(float));
memmove(currV_samples, currV_samples + 1, FILTER_SIZE * sizeof(float));
memmove(currW_samples, currW_samples + 1, FILTER_SIZE * sizeof(float));
u8 idx = current_sample_index;
// 优化计算
const float scale = ADC_SCALE * CURRENT_FACTOR;
const float offset = CURRENT_OFFSET * CURRENT_FACTOR;
currU_samples[FILTER_SIZE] =
(adc_value[2] * ADC_SCALE + CURRENT_OFFSET) * CURRENT_FACTOR;
currV_samples[FILTER_SIZE] =
(adc_value[3] * ADC_SCALE + CURRENT_OFFSET) * CURRENT_FACTOR;
currW_samples[FILTER_SIZE] =
(adc_value[4] * ADC_SCALE + CURRENT_OFFSET) * CURRENT_FACTOR;
currU_samples[idx] = adc_value[2] * scale + offset;
currV_samples[idx] = adc_value[3] * scale + offset;
currW_samples[idx] = adc_value[4] * scale + offset;
// 更新索引
current_sample_index++;
if (current_sample_index >= FILTER_SIZE - 1) {
current_sample_index = 0;
}
}
/**
Robot_V2/device/Can.cpp
+6 -1
View File
@@ -6,6 +6,7 @@
#include "gd32c10x.h"
#include "gd32c10x_can.h"
#include "gd32c10x_dma.h"
#include "systick.h"
#include <cstdio>
#include <cstdlib>
#include <cstring>
@@ -258,8 +259,12 @@ void can_loopback_init(void) {
can_parameter.time_segment_2 = CAN_BT_BS2_2TQ;
can_parameter.prescaler = 6;
if (can_init(CANX, &can_parameter) != SUCCESS) {
while (1)
u16 i = 200;
while (--i>0){
delay_1ms(10);
printf("can init error \r\n");
}
system_soft_reset();
}
/* initialize filter */
Robot_V2/device/Can.h
+21 -12
View File
@@ -17,30 +17,39 @@ typedef union {
} float_union_t;
// 添加辅助函数用于将u16类型数据打包到u8数组中
static inline void pack_u16_to_bytes(u8 * const buffer, const u16 offset, const u16 value) {
*(reinterpret_cast<u16*>(buffer + offset)) = value;
static inline void pack_u16_to_bytes(u8 *buffer, u16 offset, u16 value) {
buffer[offset] = (u8)(value & 0xFF);
buffer[offset + 1] = (u8)((value >> 8) & 0xFF);
}
// 添加辅助函数用于将float类型数据打包到u8数组中
static inline void pack_float_to_bytes(u8 * const buffer, const u16 offset,
const float_union_t &float_union) {
*(reinterpret_cast<u32*>(buffer + offset)) = *(reinterpret_cast<const u32*>(float_union.b));
static inline void pack_float_to_bytes(u8 *buffer, u16 offset,
float_union_t &float_union) {
buffer[offset] = float_union.b[0];
buffer[offset + 1] = float_union.b[1];
buffer[offset + 2] = float_union.b[2];
buffer[offset + 3] = float_union.b[3];
}
// 添加辅助函数用于从u8数组中解包short类型数据
static inline short unpack_bytes_to_short(const u8 * const buffer, const u16 offset) {
return *(reinterpret_cast<const short*>(buffer + offset));
static inline short unpack_bytes_to_short(u8 *buffer, u16 offset) {
short value = (short)(buffer[offset] | (buffer[offset + 1] << 8));
return value;
}
// 添加辅助函数用于从u8数组中解包u16类型数据
static inline u16 unpack_bytes_to_u16(const u8 * const buffer, const u16 offset) {
return *(reinterpret_cast<const u16*>(buffer + offset));
static inline u16 unpack_bytes_to_u16(u8 *buffer, u16 offset) {
u16 value = (u16)(buffer[offset] | (buffer[offset + 1] << 8));
return value;
}
// 添加辅助函数用于从u8数组中解包float类型数据
static inline float_union_t unpack_bytes_to_float(const u8 * const buffer, const u16 offset) {
static float_union_t float_union;
*(reinterpret_cast<u32*>(float_union.b)) = *(reinterpret_cast<const u32*>(buffer + offset));
static inline float_union_t unpack_bytes_to_float(u8 *buffer, u16 offset) {
float_union_t float_union;
float_union.b[0] = buffer[offset];
float_union.b[1] = buffer[offset + 1];
float_union.b[2] = buffer[offset + 2];
float_union.b[3] = buffer[offset + 3];
return float_union;
}
Robot_V2/device/Timer.cpp
+34 -110
View File
@@ -8,6 +8,7 @@
#include "gd32c10x_rcu.h"
#include "gd32c10x_timer.h"
#include "initial.h"
#include <cstdio>
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
@@ -43,10 +44,8 @@ void nvic_config(void) {
/* 设置NVIC优先级分组，1位抢占优先级，3位响应优先级 */
nvic_priority_group_set(NVIC_PRIGROUP_PRE2_SUB2);
/* 使能TIMER2中断，设置抢占优先级为1，子优先级为1 */
nvic_irq_enable(TIMER2_IRQn, 1, 1);
nvic_irq_enable(TIMER3_IRQn, 1, 2);
nvic_irq_enable(TIMER1_IRQn, 2,
1); // 使能TIMER1中断，抢占优先级2，子优先级1
nvic_irq_enable(TIMER2_IRQn, 0, 1);
nvic_irq_enable(TIMER1_IRQn, 0, 1);
}
/**
@@ -90,47 +89,6 @@ void TIMER2_config(void) {
timer_enable(TIMER2);
}
/**
* @brief 配置定时器3
* @return void
*
*配置定时器3，用于诊断恢复
*/
void TIMER3_config(void) {
/* ----------------------------------------------------------------------------
TIMER3 Configuration:
TIMER3CLK = SystemCoreClock/120 = 1MHz, the period is 50us(50/1000000 =
50us).
----------------------------------------------------------------------------
*/
timer_parameter_struct timer_struct;
/* 使能TIMER3时钟 */
rcu_periph_clock_enable(RCU_TIMER3);
/* 复位TIMER3外设到默认状态 */
timer_deinit(TIMER3);
/* 初始化TIMER初始化参数结构体 */
timer_struct_para_init(&timer_struct);
/* TIMER3配置参数设置 */
timer_struct.prescaler =
TIMER3_PRESCALER; /* 预分频值：120分频(从0开始计数) */
timer_struct.alignedmode = TIMER_COUNTER_EDGE; /* 边沿对齐模式 */
timer_struct.counterdirection = TIMER_COUNTER_UP; /* 向上计数模式 */
timer_struct.period =
TIMER3_PERIOD; /* 自动重装载值：50次计数(从0开始计数) */
timer_struct.clockdivision = TIMER_CKDIV_DIV1; /* 时钟分频值：1分频 */
timer_init(TIMER3, &timer_struct);
/* 清除更新中断标志位 */
timer_interrupt_flag_clear(TIMER3, TIMER_INT_FLAG_UP);
/* 使能更新中断 */
timer_interrupt_enable(TIMER3, TIMER_INT_UP);
/* 使能TIMER3 */
timer_enable(TIMER3);
}
/**
* @brief 配置TIMER2中断
* @return void
@@ -155,76 +113,71 @@ uint64_t get_system_runtime_us(void) { return system_runtime_us; }
* @return void
*/
void TIMER2_IRQHandler(void) {
static u8 mode = 0;
// 检查更新中断标志并处理
if (timer_interrupt_flag_get(TIMER2, TIMER_INT_UP) != RESET) {
// 清除更新中断标志
timer_interrupt_flag_clear(TIMER2, TIMER_INT_UP);
// 每100us执行一次的操作
total_us100_counter++;
scheduler.taskFlags |= (1 << TASK_ANGLE_UPDATE) |
(1 << TASK_VELOCITY_UPDATE) |
(1 << TASK_MOTOR_CONTROL);
switch (total_us100_counter) { // 每1ms执行的代码，负载均衡
case 1: {
case 0: {
scheduler.taskFlags |= (1 << TASK_MCU_TEMP_UPDATE);
break;
}
case 2: {
case 1: {
scheduler.taskFlags |= (1 << TASK_DRV_TEMP_UPDATE);
break;
}
case 3: {
case 2: {
scheduler.taskFlags |= (1 << TASK_VBUS_UPDATE);
break;
}
case 4: {
case 3: {
break;
}
case 9: {
CAN0_transmiter.can0_send_flag = SET;
scheduler.taskFlags |= (1 << TASK_CAN_TRANSMIT);
break;
}
case 9: {
mode = FOC.control_mode;
break;
}
}
if (total_us100_counter >= 10) {
total_us100_counter++;
if (total_us100_counter > 9) {
total_us100_counter = 0;
// 可以添加每1ms执行的代码，无负载均衡
static u8 ms10_counter = 0;
ms10_counter++;
switch (ms10_counter) { // 每10ms执行的代码，负载均衡
case 1: {
static u8 ms1_counter = 0;
switch (ms1_counter) { // 每10ms执行的代码，负载均衡
case 0: {
scheduler.taskFlags |= (1 << TASK_THERMAL_CONTROL);
break;
}
case 2: {
case 1: {
scheduler.taskFlags |= (1 << TASK_MAGNETIC_CHECK);
break;
}
case 2: {
break;
}
if (ms10_counter >= 10) {
ms10_counter = 0;
}
ms1_counter++;
if (ms1_counter > 9) {
ms1_counter = 0;
// 可以添加每10ms执行的代码，无负载均衡
static u8 ms100_counter = 0;
ms100_counter++;
switch (ms100_counter) { // 每100ms执行的代码，负载均衡
case 1: {
break;
}
if (ms100_counter >= 10) {
ms100_counter = 0;
// 可以添加每100ms执行的代码
static u8 sec_counter = 0;
sec_counter++;
if (sec_counter >= 10) {
sec_counter = 0;
// 可以添加每1s执行的代码
}
static u8 ms10_counter = 0;
ms10_counter++;
if (ms10_counter > 9) {
ms10_counter = 0;
// 可以添加每100ms执行的代码
static u8 sec_counter = 0;
sec_counter++;
if (sec_counter > 9) {
sec_counter = 0;
// 可以添加每1s执行的代码
}
}
}
@@ -232,33 +185,6 @@ void TIMER2_IRQHandler(void) {
}
}
/**
* @brief TIMER3中断处理函数
* @return void
*/
void TIMER3_IRQHandler(void) {
// 检查更新中断标志并处理
if (timer_interrupt_flag_get(TIMER3, TIMER_INT_UP) != RESET) {
// 清除更新中断标志
timer_interrupt_flag_clear(TIMER3, TIMER_INT_UP);
static u8 count = 0;
count++;
// 10ms测试代码
if (count >= 10) {
count = 0;
// 100ms测试代码
static u8 count_10 = 0;
count_10++;
if (count_10 >= 10) {
count_10 = 0;
// 1s测试代码
}
}
}
}
/**
* @brief TIMER1 GPIO配置函数
*/
@@ -343,8 +269,6 @@ void timer1_pwm_config(void) {
// 使能CH3比较中断
timer_interrupt_enable(TIMER1, TIMER_INT_CH3);
nvic_irq_enable(TIMER1_IRQn, 0,
2); // 使能TIMER1中断，抢占优先级2，子优先级1
// 启动TIMER1
timer_enable(TIMER1);
Robot_V2/device/Timer.h
-2
View File
@@ -11,8 +11,6 @@ void User_timer_init(void);
void Set_Pwm(uint16_t channel, float duty_cycle);
void Set_3Pwm(float duty_cycle0, float duty_cycle1,float duty_cycle2);
void TIMER2_IRQHandler(void);
void TIMER3_config(void);
void TIMER3_IRQHandler(void);
void timer1_pwm_config(void);
void timer1_gpio_config(void);
Robot_V2/device/adc.h
+2
View File
@@ -7,6 +7,7 @@ extern "C" {
#include "stdint.h"
constexpr u8 FILTER_SIZE = 6;
constexpr u8 BLOCK_SIZE = FILTER_SIZE * sizeof(float);
class sampler {
private:
float vbus_samples[FILTER_SIZE + 1];
@@ -15,6 +16,7 @@ class sampler {
float currW_samples[FILTER_SIZE + 1];
float mcu_temp_samples[FILTER_SIZE + 1];
float drv_temp_samples[FILTER_SIZE + 1];
u8 current_sample_index;
float tempora_sample;
Robot_V2/device/ang_sensor.cpp
+26 -16
View File
@@ -1,5 +1,6 @@
#include "ang_sensor.h"
#include "Filter.h"
#include "core_cmFunc.h"
#include "gd32c10x_dma.h"
#include "gd32c10x_gpio.h"
#include "gd32c10x_spi.h"
@@ -105,8 +106,8 @@ int spi_rw_half_word_dma(uint32_t spi_periph, uint16_t tx_data,
dma_transfer_number_config(DMA0, DMA_CH1, 1);
dma_channel_enable(DMA0, DMA_CH1);
// 等待传输完成，超时时间为1000个循环
uint32_t timeout = 1000;
// 等待传输完成，超时时间为100个循环
uint32_t timeout = 100;
while ((!spi_dma_tx_complete || !spi_dma_rx_complete) && --timeout) {
// 空循环等待DMA传输完成
}
@@ -209,22 +210,31 @@ angle_sensor::angle_sensor(void) { KTH7823_init(); }
* @brief 读取传感器数据
* @return 传感器数据
*/
float angle_sensor::read_sensorAngle(void) {
u16 u16data = FAULT_RECEIVE;
inline float angle_sensor::read_sensorAngle(void) {
uint16_t raw_data;
SPI_CS_ENABLE();
// 使用DMA发送命令并接收数据
if (!spi_rw_half_word_dma(SPI0, DEFAULT_COMMAND, &u16data)) {
SPI_CS_DISABLE();
current_angle_deg = u16data * ANGLE_FACTOR;
current_angle_rad = u16data * RAD_FACTOR;
return current_angle_deg;
} else {
SPI_CS_DISABLE();
// 处理读取错误
SPI_CS_ENABLE();
u8 result = spi_rw_half_word_dma(SPI0, DEFAULT_COMMAND, &raw_data);
SPI_CS_DISABLE();
if (result == 0) {
// 单次浮点乘法优化：只计算一次度数，然后转换为弧度
float angle_deg = (float)raw_data * ANGLE_FACTOR;
current_angle_deg = angle_deg;
current_angle_rad = angle_deg * DEG2RAD; // M_PI/180.0f
return angle_deg;
}
#ifdef DEBUG
// 仅在调试模式下输出错误，避免发布版本中的I/O开销
if (result != 0) {
// 使用更高效的串口输出替代printf（如果可用）
printf("Error reading angle from sensor.\n");
return FAULT_RECEIVE; // 错误返回
}
}
#endif
return (float)FAULT_RECEIVE;
}
/**
* @brief 更新传感器角度（输出级）
Robot_V2/main.cpp
+13 -12
View File
@@ -3,41 +3,41 @@
#include "DengFOC.h"
#include "Filter.h"
#include "Timer.h"
#include "adc.h"
#include "ang_sensor.h"
#include "core_cm4.h"
#include "core_cmFunc.h"
#include "health.h"
#include "initial.h"
#include "sys/health.h"
#include "sys/systick.h"
#include "systick.h"
#include "ang_sensor.h"
#include "adc.h"
TaskScheduler scheduler = {0};
int main(void) {
systick_config(); // 系统时钟初始化
USART0_init(); // 调试串口初始化
User_timer_init(); // Timer初始化
driver_init(); // 电机驱动初始化
systick_config(); // 系统时钟初始化
USART0_init(); // 调试串口初始化
User_timer_init(); // Timer初始化
driver_init(); // 电机驱动初始化
extern FOC_Controlroller FOC; // FOC控制
extern sampler sampler; // 采样器
extern EncoderVelocityKalman Vel_kalman; // 速度滤波
extern EncoderAngleKalman Angle_kalman; // 角度滤波
extern EncoderAngleKalman Angle_kalman; // 角度滤波
extern angle_sensor KTH7823; // 角度传感器
extern CAN_transmiter CAN0_transmiter; // CAN
extern ThermalController thermalController; // 热量控制器
delay_1ms(5);
angsens_MG_check(); // 角度传感器磁感应强度测试
watchdog_init(5000); // 看门狗初始化
angsens_MG_check(); // 角度传感器磁感应强度测试
watchdog_init(5000); // 看门狗初始化
while (1) {
fwdgt_counter_reload(); // 看门狗复位
__disable_irq();
// 保存当前任务标志快照以提高效率并确保一致性
u32 currentTasks = scheduler.taskFlags;
// 一次性清除所有已处理的任务标志
scheduler.taskFlags &= ~currentTasks;
__enable_irq();
// 按照优先级和重要性处理任务
if (currentTasks & (1 << TASK_CURRENT_UPDATE)) { // 更新电流采样值
@@ -94,6 +94,7 @@ int main(void) {
}
if (currentTasks & (1 << TASK_CAN_TRANSMIT)) { // CAN通信
CAN0_transmiter.communication_check();
fwdgt_counter_reload(); // 看门狗复位
}
}
}
Robot_V2/sys/initial.cpp
-1
View File
@@ -39,7 +39,6 @@ u8 watchdog_init(u32 time) {
void driver_init(void) {
timer1_gpio_config();
timer1_pwm_config();
TIMER3_config();
}
int fputc(int ch, FILE *f) throw() {