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

fix(FOC): 修正大角度下会导致角度控制失控的问题

Browse Source 
移除冗余的 `_electricalAngle` 函数，修正 `setPhaseVoltage` 中电气角度的计算方式
在定时器中断中增加实时角度更新，支持新增的无 PID 角度控制模式。
This commit is contained in:
cbb
2025-11-10 02:08:03 +08:00
parent b33574d392
commit 7a602dda48
6 changed files with 35 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Robot_V2/device/ADC.cpp
+6 -4
View File
@@ -1,12 +1,13 @@
#include "adc.h"
#include "Filter.h"
#include "Can.h"
#include "gd32c10x_can.h"
#include "gd32c10x_eval.h"
#include "math.h"
#include <cstring>
#include "gd32c10x_eval.h"
#include <gd32c10x_dma.h>
#include <stdint.h>
constexpr float ERROR_ADC_VAL = 255.0f;
constexpr float ADC_SCALE = 3.3f / 4096.0f;
constexpr float TEMP_FACTOR = 1000.0f / 4.3f;
@@ -17,10 +18,11 @@ constexpr float CURRENT_FACTOR =
1.0f / 50.0f * 100.0f; // 50倍放大，100倍电流采样电阻, 单位mA
constexpr float MAX_TEMP = 150.0f;
constexpr float MIN_TEMP = -40.0f;
constexpr float MAX_CURRENT = 65.0f;
constexpr float MIN_CURRENT = -0.1f;
constexpr float MAX_CURRENT = 40.0f;
constexpr float MIN_CURRENT = -40.0f;
constexpr float MAX_VCC = 65.0f;
constexpr float MIN_VCC = -0.1f;
extern CAN_transmiter CAN0_transmiter;
u16 adc_value[6] = {0}; // 显式初始化数组元素为0
/*!
Robot_V2/device/Timer.cpp
+4 -1
View File
@@ -173,11 +173,14 @@ void TIMER2_IRQHandler(void) {
sampler.update_current(); // 更新电流采样值
KTH7823.update(); // 更新角度传感器值
FOC.rt_velocity = Vel_kalman.update(KTH7823.getVelocity());
FOC.rt_angle = KTH7823.getAngle();
if (FOC.control_mode == 1) {
FOC.angle_control(FOC.target_angle); // 角度闭环控制
} else if (FOC.control_mode == 2) {
FOC.velocity_control(FOC.target_velocity);// 速度闭环控制
FOC.velocity_control(FOC.target_velocity); // 速度闭环控制
} else if (FOC.control_mode == 3) {
FOC.angle_control_NPID(FOC.target_angle); // 角度无PID控制
} else if (FOC.control_mode == 4) {
FOC.velocity_open_loop(); // 速度开环控制
}
Robot_V2/device/ang_sensor.cpp
+2 -2
View File
@@ -15,7 +15,7 @@ constexpr float RAD_FACTOR = 1.0f / 65536.0f * 2.0f * 3.14159265358979323846f;
constexpr u16 DEFAULT_COMMAND = 0x0000;
constexpr u16 FAULT_RECEIVE = 0xFFFF;
constexpr float VELOCITY_SCALE = 1e4f;
constexpr float ANGLE_WRAP_THRESHOLD = 270.0f;
constexpr float ANGLE_WRAP_THRESHOLD = 300.0f;
constexpr float ANGLE_WRAP_VALUE = 360.0f;
constexpr int SIZE_FLOAT = sizeof(float);
@@ -212,7 +212,7 @@ float angle_sensor::read_sensorAngle(void) {
SPI_CS_DISABLE();
current_angle_deg = u16data * ANGLE_FACTOR;
current_angle_rad = u16data * RAD_FACTOR;
return u16data * ANGLE_FACTOR;
return current_angle_deg;
} else {
SPI_CS_DISABLE();
// 处理读取错误