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/Algorithm/DengFOC.cpp
+16 -15
View File
@@ -52,10 +52,6 @@ inline float _normalizeAngle(float angle) {
// 取余运算可以用于归一化，列出特殊值例子算便知
return a >= 0 ? a : (a + 2 * PI);
}
inline float _electricalAngle() {
return _normalizeAngle((float)(DIR * PP) * KTH7823.read_sensorAngle() *
-zero_electric_angle);
}
/**
* @brief 电机控制类构造函数
*
@@ -71,7 +67,8 @@ FOC_Controlroller::FOC_Controlroller() { ; }
*/
inline void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud) {
// 计算电气角度
float angle_el = _normalizeAngle(shaft_angle * PP + zero_electric_angle);
float angle_el = _normalizeAngle(shaft_angle * PP);
// 帕克逆变换 (直接计算，避免函数调用开销)
const float sin_el = sin(angle_el);
const float cos_el = cos(angle_el);
@@ -164,9 +161,11 @@ float FOC_Controlroller::velocity_open_loop() {
* @return float: 当前角度
*/
float FOC_Controlroller::angle_control(float target_angle) {
shaft_angle = KTH7823.getRawAngle() * DEG2RAD;
// PID 控制计算
float Uq = angle_loop(shaft_angle - target_angle * ANGLE_FACTOR);
float Uq = angle_loop(target_angle - rt_angle);
// 获取电机轴角度
shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD;
// 设置相电压
setPhaseVoltage(Uq, 0.0f);
return shaft_angle;
@@ -179,13 +178,13 @@ float FOC_Controlroller::angle_control(float target_angle) {
* @return float: 当前角度
*/
float FOC_Controlroller::angle_control_NPID(float target_angle) {
shaft_angle = KTH7823.getRawAngle() * DEG2RAD;
float Kp = 20.0f;
static float Kp = 20.0f;
// 控制力矩计算
float Uq =
1.0f -
_constrain(Kp * (target_angle * ANGLE_FACTOR - DIR * shaft_angle),
-voltage_power_supply, voltage_power_supply);
float Uq = _constrain(Kp * (target_angle - rt_angle), -voltage_power_supply,
voltage_power_supply);
// 获取电机轴角度
shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD;
// 设置相电压
setPhaseVoltage(Uq, 0.0f);
return shaft_angle;
@@ -198,9 +197,11 @@ float FOC_Controlroller::angle_control_NPID(float target_angle) {
* @return float: 当前速度
*/
float FOC_Controlroller::velocity_control(float target_velocity) {
shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD;
// PID 控制计算
float Uq = vel_loop(rt_velocity - target_velocity);
float Uq = vel_loop(target_velocity - rt_velocity);
// 获取电机轴角度
shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD;
// 设置相电压
setPhaseVoltage(Uq, 0.0f);
return rt_velocity;
Robot_V2/Algorithm/DengFOC.h
+4 -4
View File
@@ -35,12 +35,12 @@ class FOC_Controlroller {
float shaft_angle = 0;
float Ualpha, Ubeta = 0, Ua = 0, Ub = 0, Uc = 0;
float zero_electric_angle = 0;
float voltage_power_supply = 24.0f; // 电源电压
float voltage_power_supply = 24.0f; // 电源电压
float v_half = voltage_power_supply * 0.5f; // 电源电压的一半
PIDController vel_loop =
PIDController{1.5f, 1.0f, 0.0005, 100000, voltage_power_supply};
PIDController angle_loop = PIDController{140.0f, 80.0f, 0.005f, 100000, voltage_power_supply};
PIDController vel_loop =PIDController{1.75f, 1.0f, 0.0002f, 1000000, voltage_power_supply};
PIDController angle_loop =
PIDController{140.0f, 80.0f, 0.005f, 10000000, voltage_power_supply};
void setPhaseVoltage(float Uq, float Ud);
};
Robot_V2/Algorithm/pid.cpp
+3 -2
View File
@@ -12,7 +12,7 @@ PIDController::PIDController(float P, float I, float D, float ramp, float limit)
,
limit(limit) // PID控制器输出限幅
,
error_prev(0.0f), output_prev(0.0f), integral_prev(0.0f), timestamp_prev(0) {}
error_prev(0.0f), output_prev(0.0f), integral_prev(0.0f) {}
// PID 控制器函数
float PIDController::operator()(float error) {
@@ -29,6 +29,7 @@ float PIDController::operator()(float error) {
// 将P,I,D三环的计算值加起来
float output = proportional + integral + derivative;
CAN0_transmiter.testfloat_union.f = D;
output = _constrain(output, -limit, limit);
// 输出斜坡限制
@@ -40,7 +41,7 @@ float PIDController::operator()(float error) {
else if (output_rate < -output_ramp)
output = output_prev - output_ramp * TIME_SLICE;
}
CAN0_transmiter.testfloat_union.f = output;
// 保存值（为了下一次循环）
integral_prev = integral;
output_prev = output;
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();
// 处理读取错误