From 7a602dda489fdeb05d65598bead9fe9614168122 Mon Sep 17 00:00:00 2001 From: cbb <2625274797@qq.com> Date: Mon, 10 Nov 2025 02:08:03 +0800 Subject: [PATCH] =?UTF-8?q?fix(FOC):=20=E4=BF=AE=E6=AD=A3=E5=A4=A7?= =?UTF-8?q?=E8=A7=92=E5=BA=A6=E4=B8=8B=E4=BC=9A=E5=AF=BC=E8=87=B4=E8=A7=92?= =?UTF-8?q?=E5=BA=A6=E6=8E=A7=E5=88=B6=E5=A4=B1=E6=8E=A7=E7=9A=84=E9=97=AE?= =?UTF-8?q?=E9=A2=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 移除冗余的 `_electricalAngle` 函数，修正 `setPhaseVoltage` 中电气角度的计算方式 在定时器中断中增加实时角度更新，支持新增的无 PID 角度控制模式。 --- Robot_V2/Algorithm/DengFOC.cpp | 31 ++++++++++++++++--------------- Robot_V2/Algorithm/DengFOC.h | 8 ++++---- Robot_V2/Algorithm/pid.cpp | 5 +++-- Robot_V2/device/ADC.cpp | 10 ++++++---- Robot_V2/device/Timer.cpp | 5 ++++- Robot_V2/device/ang_sensor.cpp | 4 ++-- 6 files changed, 35 insertions(+), 28 deletions(-) diff --git a/Robot_V2/Algorithm/DengFOC.cpp b/Robot_V2/Algorithm/DengFOC.cpp index b1c0e1d..77a7e86 100644 --- a/Robot_V2/Algorithm/DengFOC.cpp +++ b/Robot_V2/Algorithm/DengFOC.cpp @@ -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; diff --git a/Robot_V2/Algorithm/DengFOC.h b/Robot_V2/Algorithm/DengFOC.h index c1e2aa5..da9b995 100644 --- a/Robot_V2/Algorithm/DengFOC.h +++ b/Robot_V2/Algorithm/DengFOC.h @@ -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); }; diff --git a/Robot_V2/Algorithm/pid.cpp b/Robot_V2/Algorithm/pid.cpp index b23ba74..0f3af8c 100644 --- a/Robot_V2/Algorithm/pid.cpp +++ b/Robot_V2/Algorithm/pid.cpp @@ -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; diff --git a/Robot_V2/device/ADC.cpp b/Robot_V2/device/ADC.cpp index 0b1d412..87a8d39 100644 --- a/Robot_V2/device/ADC.cpp +++ b/Robot_V2/device/ADC.cpp @@ -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 -#include "gd32c10x_eval.h" #include #include - 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 /*! diff --git a/Robot_V2/device/Timer.cpp b/Robot_V2/device/Timer.cpp index 4275496..2d3651f 100644 --- a/Robot_V2/device/Timer.cpp +++ b/Robot_V2/device/Timer.cpp @@ -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(); // 速度开环控制 } diff --git a/Robot_V2/device/ang_sensor.cpp b/Robot_V2/device/ang_sensor.cpp index cd9db6a..0ce93fd 100644 --- a/Robot_V2/device/ang_sensor.cpp +++ b/Robot_V2/device/ang_sensor.cpp @@ -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(); // 处理读取错误