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

refactor(FOC): 移除代码冲突标记并优化FOC控制器实现

Browse Source 
- 移除了所有git冲突标记（<<<<<<< HEAD, =======, >>>>>>>）
- 清理了校准步骤相关注释代码
- 修复了Iq,Id计算中的角度处理逻辑
- 优化了PID控制器参数设置和初始化流程
- 重构了SVPWM相电压设置方法，移除了重复的相电压设置函数
- 修复了关闭功能和电源电压安全限制的计算

feat(CAN): 增强CAN通信协议支持多种控制模式

- 实现了目标电流、速度、角度的多参数控制
- 添加了PID参数动态配置功能
- 支持7种不同控制模式的切换
- 实现了控制模式切换时PID状态重置机制

refactor(pid): 扩展PID控制器功能接口

- 添加了set_P方法替代原有的set_P_percent
- 新增set_limit方法用于输出限幅控制
- 实现了reset方法用于PID状态重置
- 优化了PID参数设置接口

fix(timer): 优化定时器任务调度机制

- 重构了任务调度的时间片管理逻辑
- 实现了基于查找表的任务分配方式
- 优化了100us、1ms、10ms、100ms等不同周期的任务调度
- 提高了系统实时性表现

perf(config): 优化系统配置和数学常量定义

- 添加了必要的GD32头文件包含
- 重新组织了PID参数命名规范
- 定义了常用数学常量如三角函数相关数值
- 优化了定时器频率和周期计算公式
This commit is contained in:
cbb
2026-03-09 01:48:12 +08:00
parent 1ee2f68a89
commit 2b93a4537c
8 changed files with 212 additions and 399 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Robot_V2/Algorithm/FOC.cpp
+38 -272
View File
@@ -25,7 +25,6 @@ extern CurrentKalman Current_kalman;
#define constrain(value, min, max, target_min, target_max) \ #define constrain(value, min, max, target_min, target_max) \
((value - min) / (max - min) * (target_max - target_min)) ((value - min) / (max - min) * (target_max - target_min))
<<<<<<< HEAD
// 校准步骤（代码模板） // 校准步骤（代码模板）
void calibrate_zero_position(void) { void calibrate_zero_position(void) {
// Set_3Pwm(.25, 0, 0); // 起劲 // Set_3Pwm(.25, 0, 0); // 起劲
@@ -50,23 +49,14 @@ void calibrate_zero_position(void) {
// Set_3Pwm(0, 0, 0); // 停止 // Set_3Pwm(0, 0, 0); // 停止
} }
=======
>>>>>>> c9934849c58e5b5daedd91d8bfde3d56aa02f83f
// 通过Ia,Ib,Ic计算Iq,Id(目前仅输出Iq) // 通过Ia,Ib,Ic计算Iq,Id(目前仅输出Iq)
inline float FOC_Controller::cal_Iq_Id(float current_a, float current_b, inline float FOC_Controller::cal_Iq_Id(float current_a, float current_b,
float current_c) { float current_c) {
// 计算电气角度 // 计算电气角度
<<<<<<< HEAD
float angle_el = fmod(shaft_angle * PP - KTH7823.calibration_angle, 360.0f); float angle_el = fmod(shaft_angle * PP - KTH7823.calibration_angle, 360.0f);
=======
float angle_el = fmod(shaft_angle * PP, 360.0f);
>>>>>>> c9934849c58e5b5daedd91d8bfde3d56aa02f83f
// Clarke变换 (使用三相电流) // Clarke变换 (使用三相电流)
<<<<<<< HEAD
float I_alpha, I_beta; float I_alpha, I_beta;
=======
>>>>>>> c9934849c58e5b5daedd91d8bfde3d56aa02f83f
I_alpha = (2.0f / 3.0f) * current_a - (1.0f / 3.0f) * current_b - I_alpha = (2.0f / 3.0f) * current_a - (1.0f / 3.0f) * current_b -
(1.0f / 3.0f) * current_c; (1.0f / 3.0f) * current_c;
I_beta = _1_SQRT3 * current_b - _1_SQRT3 * current_c; I_beta = _1_SQRT3 * current_b - _1_SQRT3 * current_c;
@@ -76,17 +66,11 @@ inline float FOC_Controller::cal_Iq_Id(float current_a, float current_b,
arm_sin_cos_f32(angle_el, &sin_el, &cos_el); arm_sin_cos_f32(angle_el, &sin_el, &cos_el);
float I_q, I_d; float I_q, I_d;
I_q = I_beta * cos_el - I_alpha * sin_el; I_q = I_beta * cos_el - I_alpha * sin_el;
<<<<<<< HEAD
I_d = I_beta * sin_el + I_alpha * cos_el; I_d = I_beta * sin_el + I_alpha * cos_el;
// 返回Iq值，乘以1000以转换为mA // 返回Iq值，乘以1000以转换为mA
return Current_kalman.update(I_q * 1000 - KE_FACTOR * rt_velocity); return Current_kalman.update(I_q * 1000 - KE_FACTOR * rt_velocity);
// return I_q * 1000 - KE_FACTOR * rt_velocity; // return I_q * 1000 - KE_FACTOR * rt_velocity;
=======
return I_q * 1000.0f -
KE_FACTOR * rt_velocity; // 返回Iq值，乘以1000以转换为mA
>>>>>>> c9934849c58e5b5daedd91d8bfde3d56aa02f83f
} }
/** /**
@@ -96,7 +80,6 @@ inline float FOC_Controller::cal_Iq_Id(float current_a, float current_b,
*/ */
u8 FOC_Controller::init() { u8 FOC_Controller::init() {
motor_init(); // 电机驱动初始化 motor_init(); // 电机驱动初始化
<<<<<<< HEAD
FOC.cur_loop_p = DEFAULT_current_loop_p * 100 / limit_voltage; FOC.cur_loop_p = DEFAULT_current_loop_p * 100 / limit_voltage;
FOC.cur_MIT_p = DEFAULT_current_MIT_p * 100 / limit_voltage; FOC.cur_MIT_p = DEFAULT_current_MIT_p * 100 / limit_voltage;
@@ -106,22 +89,6 @@ u8 FOC_Controller::init() {
FOC.ang_loop_p = DEFAULT_angle_loop_p * 100 / limit_voltage; FOC.ang_loop_p = DEFAULT_angle_loop_p * 100 / limit_voltage;
FOC.ang_angloop_p = DEFAULT_angle_angloop_p * 100 / limit_voltage; FOC.ang_angloop_p = DEFAULT_angle_angloop_p * 100 / limit_voltage;
FOC.ang_MIT_p = DEFAULT_angle_MIT_p * 100 / limit_voltage; FOC.ang_MIT_p = DEFAULT_angle_MIT_p * 100 / limit_voltage;
=======
vel_loop.set_PID(DEFAULT_vel_loop_p * 100 / voltage_power_supply, vel_loop_Ki,
vel_loop_Kd); // 设置电机速度控制参数
angle_loop.set_PID(DEFAULT_angle_loop_p * 100 / voltage_power_supply, 100.0f,
0.2f); // 设置电机角度控制参数
current_loop.set_PID(DEFAULT_current_loop_p * 100 / voltage_power_supply, 5e-7f,
7e-8); // 设置电机电流控制参数
vel_inVelAngloop.set_PID(DEFAULT_vel_angloop_p * 100, vel_angloop_Ki,
vel_angloop_Kd); // 设置电机速度控制参数
ang_inVelAngloop.set_PID(DEFAULT_ang_angloop_p * 100 / voltage_power_supply,
ang_angloop_Ki,
ang_angloop_Kd); // 设置电机角度控制参数
>>>>>>> c9934849c58e5b5daedd91d8bfde3d56aa02f83f
CAN0_transmiter.testfloat_union.f = DEFAULT_ang_angloop_p * 100 / voltage_power_supply;
return 0; return 0;
} }
@@ -131,150 +98,49 @@ u8 FOC_Controller::init() {
*/ */
FOC_Controller::FOC_Controller() { ; } FOC_Controller::FOC_Controller() { ; }
/** // /**
* @brief 设置电机输出 // * @brief 设置电机输出
* // *
* @param Uq: q轴电压 // * @param Uq: q轴电压
* @param Ud: d轴电压 // * @param Ud: d轴电压
* @param angle_el: 电机轴角度 // * @param angle_el: 电机轴角度
*/ // */
inline void FOC_Controller::setPhaseVoltage(float Uq, float Ud) { // inline void FOC_Controller::setPhaseVoltage(float Uq, float Ud) {
// 计算电气角度 // // 计算电气角度
float angle_el = fmod(shaft_angle * PP, 360.0f); // float angle_el = fmod(shaft_angle * PP, 360.0f);
// 帕克逆变换 (直接计算，避免函数调用开销) // // 帕克逆变换 (直接计算，避免函数调用开销)
float sin_el, cos_el; // float sin_el, cos_el;
arm_sin_cos_f32(angle_el, &sin_el, &cos_el); // arm_sin_cos_f32(angle_el, &sin_el, &cos_el);
float Ualpha = Ud * cos_el - Uq * sin_el; // float Ualpha = Ud * cos_el - Uq * sin_el;
float Ubeta = Ud * sin_el + Uq * cos_el; // float Ubeta = Ud * sin_el + Uq * cos_el;
// 克拉克逆变换 (使用预计算常量优化) // // 克拉克逆变换 (使用预计算常量优化)
Ua = Ualpha + v_half; // Ua = Ualpha + v_half;
Ub = SQRT3DV2 * Ubeta - 0.5f * Ualpha + v_half; // Ub = SQRT3DV2 * Ubeta - 0.5f * Ualpha + v_half;
Uc = -0.5f * Ualpha - SQRT3DV2 * Ubeta + v_half; // Uc = -0.5f * Ualpha - SQRT3DV2 * Ubeta + v_half;
if (Ua > v_safe) // if (Ua > v_safe)
Ua = v_safe; // Ua = v_safe;
else if (Ua < -v_safe) // else if (Ua < -v_safe)
Ua = -v_safe; // Ua = -v_safe;
if (Ub > v_safe) // if (Ub > v_safe)
Ub = v_safe; // Ub = v_safe;
else if (Ub < -v_safe) // else if (Ub < -v_safe)
Ub = -v_safe; // Ub = -v_safe;
if (Uc > v_safe) // if (Uc > v_safe)
Uc = v_safe; // Uc = v_safe;
else if (Uc < -v_safe) // else if (Uc < -v_safe)
Uc = -v_safe; // Uc = -v_safe;
// 设置PWM输出 // // 设置PWM输出
float voltage_exchange = limit_voltage / v_safe; // float voltage_exchange = limit_voltage / v_safe;
duty_a = Ua / voltage_power_supply * voltage_exchange; // duty_a = Ua / voltage_power_supply * voltage_exchange;
duty_b = Ub / voltage_power_supply * voltage_exchange; // duty_b = Ub / voltage_power_supply * voltage_exchange;
duty_c = Uc / voltage_power_supply * voltage_exchange; // duty_c = Uc / voltage_power_supply * voltage_exchange;
<<<<<<< HEAD // Set_3Pwm(duty_a, duty_b, duty_c);
======= // }
Set_3Pwm(duty_a, duty_b, duty_c);
}
/**
* @brief 使用SVPWM算法设置电机输出（优化版）
*
* @param Uq: q轴电压
* @param Ud: d轴电压
*/
inline void FOC_Controller::setSVPWM_PhaseVoltage(float Uq, float Ud) {
// 计算电气角度（保持原有角度单位和范围）
float angle_el = fmod(shaft_angle * PP, 360.0f);
// 帕克逆变换
float sin_el, cos_el;
arm_sin_cos_f32(angle_el, &sin_el, &cos_el);
float Ualpha = Ud * cos_el - Uq * sin_el;
float Ubeta = Ud * sin_el + Uq * cos_el;
// 计算角度并归一化到 [0, 2π)
float angle = atan2f(Ubeta, Ualpha);
if (angle < 0)
angle += 2.0f * PI;
// 扇区与局部角度
int sector = (int)(angle * INV_PI_3) + 1;
if (sector < 1)
sector = 1;
else if (sector > 6)
sector = 6;
float local_angle = angle - (sector - 1) * PI_3;
// 模长计算
float magnitude = sqrtf(Ualpha * Ualpha + Ubeta * Ubeta);
// 计算电压调制比，同时限制最大输出电压
float modulation_index =
(magnitude / voltage_power_supply); // 90%作为安全裕量
// 电压限制
if (limit_voltage > voltage_power_supply) {
limit_voltage = voltage_power_supply;
}
float limit_percent = limit_voltage / v_safe;
if (modulation_index > limit_percent) {
// 限制电压
modulation_index = limit_percent;
// 同时相应地调整magnitude，保持电压矢量方向不变，根据调整后的调制比反推magnitude
magnitude = voltage_power_supply * limit_percent;
}
// 使用arm_sin_cos_f32计算local_angle的sin和cos
float sin_l, cos_l;
arm_sin_cos_f32(local_angle, &sin_l, &cos_l);
float T1 = modulation_index * (SQRT3DV2 * cos_l - COS_PI_3 * sin_l);
float T2 = modulation_index * sin_l;
float T0 = 1.0f - T1 - T2;
// 计算占空比（按规范化时间映射）
switch (sector) {
case 1:
duty_a = 0.5f * T0 + T1 + T2;
duty_b = 0.5f * T0 + T2;
duty_c = 0.5f * T0;
break;
case 2:
duty_a = 0.5f * T0 + T1;
duty_b = 0.5f * T0 + T1 + T2;
duty_c = 0.5f * T0;
break;
case 3:
duty_a = 0.5f * T0;
duty_b = 0.5f * T0 + T1 + T2;
duty_c = 0.5f * T0 + T2;
break;
case 4:
duty_a = 0.5f * T0;
duty_b = 0.5f * T0 + T1;
duty_c = 0.5f * T0 + T1 + T2;
break;
case 5:
duty_a = 0.5f * T0 + T2;
duty_b = 0.5f * T0;
duty_c = 0.5f * T0 + T1 + T2;
break;
case 6:
duty_a = 0.5f * T0 + T1 + T2;
duty_b = 0.5f * T0;
duty_c = 0.5f * T0 + T1;
break;
default:
duty_a = 0.0f;
duty_b = 0.0f;
duty_c = 0.0f;
break;
}
// 设置PWM输出
>>>>>>> c9934849c58e5b5daedd91d8bfde3d56aa02f83f
Set_3Pwm(duty_a, duty_b, duty_c);
}
/** /**
* @brief 使用SVPWM算法设置电机输出 * @brief 使用SVPWM算法设置电机输出
@@ -382,11 +248,7 @@ inline void FOC_Controller::setSVPWM_PhaseVoltage(float Uq, float Ud) {
* *
*/ */
void FOC_Controller::shut_down() { void FOC_Controller::shut_down() {
<<<<<<< HEAD
drv_stop(); drv_stop();
=======
is_shut_down = 1;
>>>>>>> c9934849c58e5b5daedd91d8bfde3d56aa02f83f
motor_sleep(); motor_sleep();
is_shut_down = true; is_shut_down = true;
} }
@@ -450,11 +312,7 @@ void FOC_Controller::update_power(float voltage) {
voltage_error = false; voltage_error = false;
voltage_power_supply = voltage; voltage_power_supply = voltage;
v_half = voltage * 0.5f; v_half = voltage * 0.5f;
<<<<<<< HEAD
v_safe = voltage * 1.1f; v_safe = voltage * 1.1f;
=======
v_safe = voltage * 1.05f;
>>>>>>> c9934849c58e5b5daedd91d8bfde3d56aa02f83f
} else { } else {
voltage_error = true; voltage_error = true;
} }
@@ -469,7 +327,6 @@ void FOC_Controller::update_power(float voltage) {
float FOC_Controller::velocity_open_loop() { float FOC_Controller::velocity_open_loop() {
// 通过时间间隔和目标速度计算需要转动的机械角度，存储在shaft_angle中，在此之前，还需要对轴角度归一化 // 通过时间间隔和目标速度计算需要转动的机械角度，存储在shaft_angle中，在此之前，还需要对轴角度归一化
shaft_angle = fmod(shaft_angle + target_velocity * REDUCTION_RATIO * 1.0 / shaft_angle = fmod(shaft_angle + target_velocity * REDUCTION_RATIO * 1.0 /
<<<<<<< HEAD
MOTOR_FREQUENCY / 2, MOTOR_FREQUENCY / 2,
360.0f); 360.0f);
// 获取电机轴角度 // 获取电机轴角度
@@ -481,19 +338,6 @@ float FOC_Controller::velocity_open_loop() {
// 因为在开环控制中，Ud始终为0，只控制Uq // 因为在开环控制中，Ud始终为0，只控制Uq
const float Uq = voltage_power_supply / 2.0f; const float Uq = voltage_power_supply / 2.0f;
setSVPWM_PhaseVoltage(Uq, 0.0f); setSVPWM_PhaseVoltage(Uq, 0.0f);
=======
TIMER2_FREQUENCY,
360.0f);
// 因为在开环控制中，Ud始终为0，只控制Uq
const float Uq = voltage_power_supply / 2.0f;
setSVPWM_PhaseVoltage(Uq, 0.0f);
// CAN0_transmiter.testfloat_union.f = Uq;
// cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
// sampler.get_current('W'));
>>>>>>> c9934849c58e5b5daedd91d8bfde3d56aa02f83f
return limit_voltage; return limit_voltage;
} }
@@ -513,84 +357,6 @@ float FOC_Controller::angle_control() {
sampler.get_current('W')); sampler.get_current('W'));
// 设置相电压 // 设置相电压
setSVPWM_PhaseVoltage(Uq, 0.0f); setSVPWM_PhaseVoltage(Uq, 0.0f);
<<<<<<< HEAD
=======
return shaft_angle;
}
/**
* @brief 带速度限制的角度闭环控制
*
* @param target_angle: 目标角度
* @param max_velocity: 最大速度限制 (单位: rad/s)
* @return float: 当前角度
*/
float FOC_Controller::angle_control_with_velocity_limit() {
// 计算角度误差
float angle_error = target_angle - rt_angle;
// 使用角度PID控制器计算目标速度
float target_velocity = ang_inVelAngloop(angle_error);
// 使用速度PID控制器计算最终输出电压
// 根据目标速度大小，选择不同的PID参数
if (target_velocity < -120.0f) {
vel_inVelAngloop.set_P(vel_angloop_p * 2.0f);
} else if (target_velocity < -30.0f) {
vel_inVelAngloop.set_P(vel_angloop_p * 1.5f);
} else {
vel_inVelAngloop.set_P(vel_angloop_p);
}
float Uq = vel_inVelAngloop(rt_velocity - target_velocity);
rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
sampler.get_current('W'));
// 获取电机轴角度
shaft_angle = KTH7823.getcurrent_sensorangle();
// 设置相电压
setSVPWM_PhaseVoltage(Uq, 0.0f);
return shaft_angle;
}
/**
* @brief 带速度限制的角度闭环控制
*
* @param target_angle: 目标角度
* @param max_velocity: 最大速度限制 (单位: rad/s)
* @return float: 当前角度
*/
float FOC_Controller::MIT_Mode() {
// 第一层：角度环（外环）
// 根据目标角度和实际角度的差值，通过角度环PID控制器计算目标速度
float angle_error = rt_angle - target_angle;
float target_velocity =
ang_inVelAngloop(angle_error); // 使用angle_loop作为角度到速度的映射
// 第二层：速度环（中环）
// 根据目标速度和实际速度的差值，通过速度环PID控制器计算目标电流
float velocity_error = target_velocity - rt_velocity;
float target_current =
vel_inVelAngloop(velocity_error); // 使用vel_loop作为速度到电流的映射
CAN0_transmiter.testfloat_union.f = target_current;
// 第三层：电流环（内环）
// 计算当前实际电流
rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
sampler.get_current('W'));
// 通过电流环PID控制器计算输出电压
float current_error = target_current - rt_current;
float Uq = current_loop(current_error);
// 获取电机轴角度
shaft_angle = KTH7823.getcurrent_sensorangle();
// 设置相电压
setSVPWM_PhaseVoltage(Uq, 0.0f);
>>>>>>> c9934849c58e5b5daedd91d8bfde3d56aa02f83f
return shaft_angle; return shaft_angle;
} }
Robot_V2/Algorithm/FOC.h
-2
View File
@@ -63,7 +63,6 @@ class FOC_Controller {
void shut_down_thermal(void); // 过热关机 void shut_down_thermal(void); // 过热关机
void resume_from_thermal(void); // 过热恢复 void resume_from_thermal(void); // 过热恢复
void setSVPWM_PhaseVoltage(float Uq, float Ud);
// 此处默认值不作数 // 此处默认值不作数
PIDController vel_loop = PIDController vel_loop =
PIDController{DEFAULT_vel_loop_p * 100 / limit_voltage, vel_loop_Ki, PIDController{DEFAULT_vel_loop_p * 100 / limit_voltage, vel_loop_Ki,
@@ -104,7 +103,6 @@ class FOC_Controller {
float v_half = voltage_power_supply * 0.5f; // 电源电压的一半 float v_half = voltage_power_supply * 0.5f; // 电源电压的一半
float startup_ramp_val = 0.0f; // 软启动系数 float startup_ramp_val = 0.0f; // 软启动系数
float startup_ramp_step = 0.01f; // 启动斜坡步长，可调节以控制启动速度 float startup_ramp_step = 0.01f; // 启动斜坡步长，可调节以控制启动速度
void setPhaseVoltage(float Uq, float Ud);
void setSVPWM_PhaseVoltage(float Uq, float Ud); void setSVPWM_PhaseVoltage(float Uq, float Ud);
float cal_Iq_Id(float current_a, float current_b, float current_c); float cal_Iq_Id(float current_a, float current_b, float current_c);
}; };
Robot_V2/Algorithm/pid.cpp
+22 -2
View File
@@ -6,7 +6,6 @@
#include <algorithm> #include <algorithm>
#include <cmath> #include <cmath>
extern CAN_transmiter CAN0_transmiter; extern CAN_transmiter CAN0_transmiter;
PIDController::PIDController(float P, float I, float D, float ramp, float limit) PIDController::PIDController(float P, float I, float D, float ramp, float limit)
: P(P), I(I), D(D), output_ramp(ramp) // PID控制器加速度限幅 : P(P), I(I), D(D), output_ramp(ramp) // PID控制器加速度限幅
@@ -68,4 +67,25 @@ void PIDController::set_PID(float P, float I, float D) {
* @param P: P参数 * @param P: P参数
* @return void * @return void
*/ */
void PIDController::set_P_percent(float pp) { this->P = pp; } void PIDController::set_P(float pp) {
this->P = pp;
}
/**
* @brief 设置输出限幅
*
* @param limit: 输出限幅
* @return void
*/
void PIDController::set_limit(float limit) { this->limit = limit; }
/**
* @brief 重置PID控制器状态
*
* @return void
*/
void PIDController::reset() {
error_prev = 0.0f;
output_prev = 0.0f;
integral_prev = 0.0f;
}
Robot_V2/device/ADC.cpp
+1 -1
View File
@@ -15,7 +15,7 @@ constexpr float VCC_FACTOR = 11.0f;
constexpr float CURRENT_FACTOR = constexpr float CURRENT_FACTOR =
1.0f / 50.0f * 100.0f; // 50倍放大，100倍电流采样电阻, 单位mA 1.0f / 50.0f * 100.0f; // 50倍放大，100倍电流采样电阻, 单位mA
extern CAN_transmiter CAN0_transmiter; extern CAN_transmiter CAN0_transmiter;
extern FOC_Controlroller FOC; extern FOC_Controller FOC;
sampler sampler; // 采样器初始化 sampler sampler; // 采样器初始化
u16 adc_value[6] = {0}; // 显式初始化数组元素为0 u16 adc_value[6] = {0}; // 显式初始化数组元素为0
/*! /*!
Robot_V2/device/Can.cpp
+34 -26
View File
@@ -19,9 +19,7 @@
#include <random> #include <random>
constexpr u16 CANX = CAN0; constexpr u16 CANX = CAN0;
extern FOC_Controller FOC;
CAN_transmiter CAN0_transmiter; // CAN初始化
extern FOC_Controlroller FOC;
extern sampler sampler; extern sampler sampler;
extern angle_sensor KTH7823; extern angle_sensor KTH7823;
extern EncoderAngleKalman Angle_kalman; // 角度滤波 extern EncoderAngleKalman Angle_kalman; // 角度滤波
@@ -575,37 +573,47 @@ void CAN_transmiter::communication_check() {
// } // }
if (current_message.rx_efid == CANFD_CMD_ID) { if (current_message.rx_efid == CANFD_CMD_ID) {
// 控制模式 // 控制模式
FOC.control_mode = receive_message.rx_data[0]; FOC.control_mode = current_message.rx_data[0];
// 目标角度 // 目标电压限制
FOC.limit_voltage = FOC.limit_voltage =
float(unpack_bytes_to_u16(receive_message.rx_data, 1)) / 1000; float(unpack_bytes_to_u16(current_message.rx_data, 1)) / 1000;
// 目标电流&目标速度 // 目标电流
FOC.limit_velocity = target_current_union.f =
unpack_bytes_to_float(receive_message.rx_data, 3).f; unpack_bytes_to_float(current_message.rx_data, 3).f;
FOC.target_current = FOC.limit_velocity; FOC.current_loop.set_limit(target_current_union.f);
// 目标角度 FOC.vel_inMITloop.set_limit(target_current_union.f);
target_angle_union.f =
unpack_bytes_to_float(receive_message.rx_data, 7).f;
FOC.target_angle = target_angle_union.f;
// 目标速度 // 目标速度
target_velocity_union.f = target_velocity_union.f =
unpack_bytes_to_float(receive_message.rx_data, 11).f; unpack_bytes_to_float(current_message.rx_data, 7).f;
FOC.ang_inVelAngloop.set_limit(target_velocity_union.f);
FOC.ang_inMITloop.set_limit(target_velocity_union.f);
// 目标角度
target_angle_union.f =
unpack_bytes_to_float(current_message.rx_data, 11).f;
// 赋值
FOC.target_angle = target_angle_union.f;
FOC.target_velocity = target_velocity_union.f; FOC.target_velocity = target_velocity_union.f;
FOC.target_current = target_current_union.f; FOC.target_current = target_current_union.f;
// 解析PID参数 // 解析PID参数
float voltage = FOC.limit_voltage; float voltage = FOC.limit_voltage;
FOC.current_loop.set_P_percent(current_loop_p * P_config / voltage); // PID参数
P_config = unpack_bytes_to_u16(receive_message.rx_data, 17); u16 P_config = unpack_bytes_to_u16(current_message.rx_data, 15);
FOC.vel_loop.set_P_percent(vel_loop_p * P_config / voltage); FOC.cur_loop_p = DEFAULT_current_loop_p * P_config / voltage;
P_config = unpack_bytes_to_u16(receive_message.rx_data, 19); FOC.cur_MIT_p = DEFAULT_current_MIT_p * P_config / voltage;
FOC.angle_loop.set_P_percent(angle_loop_p * P_config / voltage);
P_config = unpack_bytes_to_u16(receive_message.rx_data, 21); P_config = unpack_bytes_to_u16(current_message.rx_data, 17);
FOC.vel_inVelAngloop.set_P_percent(vel_angloop_p * P_config / FOC.vel_loop_p = DEFAULT_vel_loop_p * P_config / voltage;
voltage); FOC.vel_angloop_p = DEFAULT_vel_angloop_p * P_config;
} else if (receive_message.rx_efid == CANFD_RESET_ID) { FOC.vel_MIT_p = DEFAULT_vel_MIT_p * P_config;
u8 first_byte = receive_message.rx_data[0];
u8 eigth_byte = receive_message.rx_data[7]; P_config = unpack_bytes_to_u16(current_message.rx_data, 19);
FOC.ang_loop_p = DEFAULT_angle_loop_p * P_config / voltage;
FOC.ang_angloop_p = DEFAULT_angle_angloop_p * P_config / voltage;
FOC.ang_MIT_p = DEFAULT_angle_MIT_p * P_config / voltage;
} else if (current_message.rx_efid == CANFD_RESET_ID) {
u8 first_byte = current_message.rx_data[0];
u8 eigth_byte = current_message.rx_data[7];
// 电机重置 // 电机重置
if (first_byte & 0x01) { if (first_byte & 0x01) {
motor_reset(); motor_reset();
Robot_V2/device/Timer.cpp
+29 -59
View File
@@ -11,7 +11,7 @@
volatile u32 total_us100_counter = 0; // 总计数器，每 100 微秒增加 1 volatile u32 total_us100_counter = 0; // 总计数器，每 100 微秒增加 1
extern u16 adc_value[7]; extern u16 adc_value[7];
extern FOC_Controlroller FOC; extern FOC_Controller FOC;
extern TaskScheduler scheduler; extern TaskScheduler scheduler;
extern CAN_transmiter CAN0_transmiter; extern CAN_transmiter CAN0_transmiter;
// 添加系统运行时间计数器 // 添加系统运行时间计数器
@@ -131,77 +131,47 @@ void TIMER2_IRQHandler(void) {
} }
timer_interrupt_flag_clear(TIMER2, TIMER_INT_UP); timer_interrupt_flag_clear(TIMER2, TIMER_INT_UP);
// 每50us执行一次的操作 - 电机控制 // 固定任务：每 50us 执行电机控制
scheduler.taskFlags |= (1 << TASK_MOTOR_CONTROL); scheduler.taskFlags |= (1u << TASK_ANGLE_UPDATE) |
scheduler.taskFlags |= (1u << TASK_VELOCITY_UPDATE) |
(1 << TASK_ANGLE_UPDATE) | (1 << TASK_VELOCITY_UPDATE); (1u << TASK_MOTOR_CONTROL);
// 使用标志位控制，每100us执行一次其他任务 // 每 100us 执行一次的任务（使用位运算替代布尔变量）
static bool isEvenCall = false; static u32 call_counter = 0;
isEvenCall = !isEvenCall; call_counter++;
if (isEvenCall) { if ((call_counter & 1u) == 0u) { // 偶数次调用（每 100us）
// 每100us执行一次的操作 - 保持原有频率 // 使用查找表设置 1ms 周期内的任务
scheduler.taskFlags |= task_schedule_100us[total_us100_counter];
switch (total_us100_counter) { // 每1ms执行的代码，负载均衡
case 0: {
scheduler.taskFlags |= (1 << TASK_MCU_TEMP_UPDATE);
break;
}
case 1: {
scheduler.taskFlags |= (1 << TASK_DRV_TEMP_UPDATE);
break;
}
case 2: {
scheduler.taskFlags |= (1 << TASK_VBUS_UPDATE);
break;
}
case 3: {
break;
}
case 9: {
scheduler.taskFlags |= (1 << TASK_CAN_TRANSMIT);
break;
}
}
total_us100_counter++; total_us100_counter++;
if (total_us100_counter > 9) { if (total_us100_counter >= 10u) {
total_us100_counter = 0; total_us100_counter = 0;
// 可以添加每1ms执行的代码，无负载均衡
static u8 ms1_counter = 0;
switch (ms1_counter) { // 每10ms执行的代码，负载均衡 // 每 1ms 执行的任务
case 0: { static u32 ms1_counter = 0;
scheduler.taskFlags |= (1 << TASK_THERMAL_CONTROL);
break;
}
case 1: {
scheduler.taskFlags |= (1 << TASK_MAGNETIC_CHECK);
break;
}
case 2: {
break;
}
}
ms1_counter++; ms1_counter++;
if (ms1_counter > 9) { if (ms1_counter >= 10u) {
ms1_counter = 0; ms1_counter = 0;
// 可以添加每10ms执行的代码，无负载均衡
static u8 ms10_counter = 0; // 每 10ms 执行的任务
static u32 ms10_counter = 0;
ms10_counter++; ms10_counter++;
if (ms10_counter > 9) { if (ms10_counter >= 10u) {
ms10_counter = 0; ms10_counter = 0;
// 可以添加每100ms执行的代码
static u8 sec_counter = 0; // 每 100ms 执行的任务
sec_counter++; static u32 ms100_counter = 0;
if (sec_counter > 9) { ms100_counter++;
sec_counter = 0; if (ms100_counter >= 10u) {
// 可以添加每1s执行的代码 ms100_counter = 0;
} // 每 1s 任务预留位置
} }
scheduler.taskFlags |= task_schedule_100ms[ms100_counter];
} }
scheduler.taskFlags |= task_schedule_10ms[ms10_counter];
} }
scheduler.taskFlags |= task_schedule_1ms[ms1_counter];
} }
} }
} }
Robot_V2/main.cpp
+25 -11
View File
@@ -95,21 +95,35 @@ void do_task(u32 currentTasks) {
if (currentTasks & (1 << TASK_MOTOR_CONTROL)) { // 电机控制任务(最高优先级) if (currentTasks & (1 << TASK_MOTOR_CONTROL)) { // 电机控制任务(最高优先级)
motor_runtime_50us++; motor_runtime_50us++;
// 函数指针数组，包含各种控制模式对应的函数 // 静态变量用于跟踪上一次的控制模式
static FOCFunctionPtr control_functions[] = { static int last_control_mode = -1;
&FOC_Controlroller::drv_hold, // 0: 占空比置
&FOC_Controlroller::
angle_control_with_velocity_limit, // 1:
// 角度闭环控制，带速度限制
&FOC_Controlroller::velocity_control, // 2: 速度闭环控制
&FOC_Controlroller::current_control, // 3: 电流闭环控制
&FOC_Controlroller::velocity_open_loop, // 4: 速度开环控制
};
// 边界检查，防止数组越界 // 边界检查，防止数组越界
if (FOC.control_mode >= 0 && FOC.control_mode < 5) { // 更新为7个控制模式 if (FOC.control_mode >= 0 &&
FOC.control_mode < 7) { // 更新为7个控制模式
// 检查控制模式是否发生变化
if (FOC.control_mode != last_control_mode) {
// 重置所有PID控制器的状态，避免积分项累积
FOC.vel_loop.reset();
FOC.angle_loop.reset();
FOC.current_loop.reset();
FOC.vel_inVelAngloop.reset();
FOC.ang_inVelAngloop.reset();
FOC.ang_inMITloop.reset();
FOC.vel_inMITloop.reset();
FOC.current_inMITloop.reset();
// 更新上次控制模式
last_control_mode = FOC.control_mode;
}
if (!FOC.is_shut_down) {
// 调用对应的控制函数 // 调用对应的控制函数
(FOC.*control_functions[FOC.control_mode])(); (FOC.*control_functions[FOC.control_mode])();
} else {
// 调用对应的控制函数
FOC.drv_stop();
}
} }
} }
if (currentTasks & (1 << TASK_MCU_TEMP_UPDATE)) { // 更新mcu温度传感器值 if (currentTasks & (1 << TASK_MCU_TEMP_UPDATE)) { // 更新mcu温度传感器值
Robot_V2/sys/config.h
+44 -7
View File
@@ -1,5 +1,6 @@
#ifndef __CONFIG_H #ifndef __CONFIG_H
#define __CONFIG_H #define __CONFIG_H
#include "gd32c10x_eval.h"
#include <cstdint> #include <cstdint>
// 如果使用CMSIS DSP库，需要定义ARM_MATH_CM4 // 如果使用CMSIS DSP库，需要定义ARM_MATH_CM4
@@ -87,17 +88,49 @@ constexpr float CURRENT_KALMAN_PROCESS_NOISE_Q = 10.0f; // 降低过程噪声
constexpr float CURRENT_KALMAN_MEASUREMENT_NOISE_R = 5e-5f; // 增大测量噪声 constexpr float CURRENT_KALMAN_MEASUREMENT_NOISE_R = 5e-5f; // 增大测量噪声
// PID配置 // PID配置
constexpr float current_loop_p = 6e-2f / 100 * 12; // 电流P constexpr float DEFAULT_current_loop_p = 0.1f / 100 * 12; // 电流P
constexpr float vel_loop_p = 0.01f / 100 * 12; // 速度P constexpr float current_loop_Ki = 0.55f; // 电流P
constexpr float angle_loop_p = 20.0f / 100 * 12; // 角度P constexpr float current_loop_Kd = 3e-4f; // 电流P
constexpr float vel_angloop_p = 0.12f / 100 * 12; // 速度 速度-角度P
constexpr float ang_angloop_p = 100.0f / 100 * 12; // 角度 速度-角度P
constexpr float DEFAULT_vel_loop_p = 0.018f / 100 * 12; // 速度P
constexpr float vel_loop_Ku = 3.600000e-03 * 0.7 * 100 / 12;
constexpr float vel_loop_Tu = 17.0f * 1e-3f;
constexpr float vel_loop_Ki = 1.2 * vel_loop_Ku / vel_loop_Tu;
constexpr float vel_loop_Kd = 0.075 * vel_loop_Ku * vel_loop_Tu * 2;
constexpr float DEFAULT_vel_angloop_p = 0.4f / 100; // 速度 速度-角度P
constexpr float vel_angloop_Ki = 5.0f; // 速度 速度-角度I
constexpr float vel_angloop_Kd = 5e-05f; // 速度 速度-角度D
constexpr float DEFAULT_angle_angloop_p = 150.0f / 100 * 12; // 角度 速度-角度P
constexpr float ang_angloop_Ki = 0.00f; // 角度 速度-角度I
constexpr float ang_angloop_Kd = 8e-04f; // 角度 速度-角度D
constexpr float DEFAULT_angle_loop_p = 60.0f / 100 * 12; // 角度P
constexpr float DEFAULT_angle_MIT_p = 100.0f / 100 * 12; // 角度 MIT P
constexpr float angle_MIT_Ki = 0.0f;
constexpr float angle_MIT_Kd = 8e-04f;
constexpr float DEFAULT_vel_MIT_p = 5.0f / 100; // 速度 MIT P
constexpr float vel_MIT_Ki = 15.0f;
constexpr float vel_MIT_Kd = 1e-04f;
constexpr float DEFAULT_current_MIT_p = 0.2f / 100 * 12; // 电流 MIT P
constexpr float current_MIT_Ki = 0.1f;
constexpr float current_MIT_Kd = 3e-4f;
// 常量定义 // 常量定义
constexpr u8 SIZE_FLOAT = sizeof(float); constexpr u8 SIZE_FLOAT = sizeof(float);
constexpr u8 SIZE_U32 = sizeof(uint32_t); constexpr u8 SIZE_U32 = sizeof(uint32_t);
constexpr float ADC_MAX = 65536.0f; constexpr float ADC_MAX = 65536.0f;
constexpr float PI = 3.14159265358979323846f; constexpr float PI = 3.14159265358979323846f;
constexpr float INV_PI_3 = 3.0f / PI;
constexpr float PI_3 = PI / 3.0f;
constexpr float _1_SQRT3 = 0.57735026919f; // 1/sqrt(3)
constexpr float _3PI_2 = 4.71238898038f;
constexpr float SQRT3 = 1.7320508075688772f;
constexpr float SQRT3DV2 = 0.8660254037844386f; // sqrt(3)/2
constexpr float COS_PI_3 = 0.5f;
constexpr float ANGLE_FACTOR = 25.0f / 180.0f * PI; constexpr float ANGLE_FACTOR = 25.0f / 180.0f * PI;
constexpr float DEG2RAD = 1.0f / 180.0f * PI; constexpr float DEG2RAD = 1.0f / 180.0f * PI;
constexpr float RAD2DEG = 180.0f / PI; // 修正RAD2DEG定义，应该是180/π constexpr float RAD2DEG = 180.0f / PI; // 修正RAD2DEG定义，应该是180/π
@@ -149,8 +182,12 @@ constexpr float ANGLESENS_FACTOR = 1.0f / ADC_MAX * 360.0f;
// TIMER2-任务调度 // TIMER2-任务调度
constexpr int TIMER2_PRESCALER = 119; // 120MHz / 120 = 1MHz constexpr int TIMER2_PRESCALER = 119; // 120MHz / 120 = 1MHz
constexpr int TIMER2_PERIOD = 49; // 1MHz / 50 = 50us constexpr int TIMER2_PERIOD = 49; // 1 / 1MHz * 50 = 50us
constexpr float TIMER2_FREQUENCY = 1 / 5e-5; constexpr int MOTOR_PERIOD = 2; // 100us/50us = 2
constexpr float TIME_SLICE =
(TIMER2_PERIOD + 1) * (TIMER2_PRESCALER + 1) / 120000000.0f; // 周期
constexpr float TIMER2_FREQUENCY = 1 / TIME_SLICE; // 频率
constexpr float MOTOR_FREQUENCY = 1 / TIME_SLICE / MOTOR_PERIOD; // 频率
// TIMER1-PWM // TIMER1-PWM
constexpr int TIMER1_PRESCALER = 0; // 不分频，计数器时钟为120MHz constexpr int TIMER1_PRESCALER = 0; // 不分频，计数器时钟为120MHz