From 2b93a4537c1259e8de00c8bae3c6c206a61413c8 Mon Sep 17 00:00:00 2001 From: cbb <2625274797@qq.com> Date: Mon, 9 Mar 2026 01:48:12 +0800 Subject: [PATCH] =?UTF-8?q?refactor(FOC):=20=E7=A7=BB=E9=99=A4=E4=BB=A3?= =?UTF-8?q?=E7=A0=81=E5=86=B2=E7=AA=81=E6=A0=87=E8=AE=B0=E5=B9=B6=E4=BC=98?= =?UTF-8?q?=E5=8C=96FOC=E6=8E=A7=E5=88=B6=E5=99=A8=E5=AE=9E=E7=8E=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - 移除了所有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参数命名规范 - 定义了常用数学常量如三角函数相关数值 - 优化了定时器频率和周期计算公式 --- Robot_V2/Algorithm/FOC.cpp | 310 +++++-------------------------------- Robot_V2/Algorithm/FOC.h | 4 +- Robot_V2/Algorithm/pid.cpp | 24 ++- Robot_V2/device/ADC.cpp | 2 +- Robot_V2/device/Can.cpp | 60 +++---- Robot_V2/device/Timer.cpp | 116 +++++--------- Robot_V2/main.cpp | 42 +++-- Robot_V2/sys/config.h | 53 ++++++- 8 files changed, 212 insertions(+), 399 deletions(-) diff --git a/Robot_V2/Algorithm/FOC.cpp b/Robot_V2/Algorithm/FOC.cpp index 58d262b..f7c167c 100644 --- a/Robot_V2/Algorithm/FOC.cpp +++ b/Robot_V2/Algorithm/FOC.cpp @@ -25,7 +25,6 @@ extern CurrentKalman Current_kalman; #define constrain(value, min, max, target_min, target_max) \ ((value - min) / (max - min) * (target_max - target_min)) -<<<<<<< HEAD // 校准步骤（代码模板） void calibrate_zero_position(void) { // Set_3Pwm(.25, 0, 0); // 起劲 @@ -50,23 +49,14 @@ void calibrate_zero_position(void) { // Set_3Pwm(0, 0, 0); // 停止 } -======= ->>>>>>> c9934849c58e5b5daedd91d8bfde3d56aa02f83f // 通过Ia,Ib,Ic计算Iq,Id(目前仅输出Iq) inline float FOC_Controller::cal_Iq_Id(float current_a, float current_b, float current_c) { // 计算电气角度 -<<<<<<< HEAD float angle_el = fmod(shaft_angle * PP - KTH7823.calibration_angle, 360.0f); -======= - float angle_el = fmod(shaft_angle * PP, 360.0f); ->>>>>>> c9934849c58e5b5daedd91d8bfde3d56aa02f83f // Clarke变换 (使用三相电流) -<<<<<<< HEAD float I_alpha, I_beta; -======= ->>>>>>> c9934849c58e5b5daedd91d8bfde3d56aa02f83f I_alpha = (2.0f / 3.0f) * current_a - (1.0f / 3.0f) * current_b - (1.0f / 3.0f) * 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); float I_q, I_d; I_q = I_beta * cos_el - I_alpha * sin_el; -<<<<<<< HEAD I_d = I_beta * sin_el + I_alpha * cos_el; // 返回Iq值，乘以1000以转换为mA return Current_kalman.update(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() { motor_init(); // 电机驱动初始化 -<<<<<<< HEAD FOC.cur_loop_p = DEFAULT_current_loop_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_angloop_p = DEFAULT_angle_angloop_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; } @@ -131,150 +98,49 @@ u8 FOC_Controller::init() { */ FOC_Controller::FOC_Controller() { ; } -/** - * @brief 设置电机输出 - * - * @param Uq: q轴电压 - * @param Ud: d轴电压 - * @param angle_el: 电机轴角度 - */ -inline void FOC_Controller::setPhaseVoltage(float Uq, float Ud) { - // 计算电气角度 - float angle_el = fmod(shaft_angle * PP, 360.0f); +// /** +// * @brief 设置电机输出 +// * +// * @param Uq: q轴电压 +// * @param Ud: d轴电压 +// * @param angle_el: 电机轴角度 +// */ +// inline void FOC_Controller::setPhaseVoltage(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; +// // 帕克逆变换 (直接计算，避免函数调用开销) +// 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; - // 克拉克逆变换 (使用预计算常量优化) - Ua = Ualpha + v_half; - Ub = SQRT3DV2 * Ubeta - 0.5f * Ualpha + v_half; - Uc = -0.5f * Ualpha - SQRT3DV2 * Ubeta + v_half; +// // 克拉克逆变换 (使用预计算常量优化) +// Ua = Ualpha + v_half; +// Ub = SQRT3DV2 * Ubeta - 0.5f * Ualpha + v_half; +// Uc = -0.5f * Ualpha - SQRT3DV2 * Ubeta + v_half; - if (Ua > v_safe) - Ua = v_safe; - else if (Ua < -v_safe) - Ua = -v_safe; - if (Ub > v_safe) - Ub = v_safe; - else if (Ub < -v_safe) - Ub = -v_safe; - if (Uc > v_safe) - Uc = v_safe; - else if (Uc < -v_safe) - Uc = -v_safe; +// if (Ua > v_safe) +// Ua = v_safe; +// else if (Ua < -v_safe) +// Ua = -v_safe; +// if (Ub > v_safe) +// Ub = v_safe; +// else if (Ub < -v_safe) +// Ub = -v_safe; +// if (Uc > v_safe) +// Uc = v_safe; +// else if (Uc < -v_safe) +// Uc = -v_safe; - // 设置PWM输出 - float voltage_exchange = limit_voltage / v_safe; - duty_a = Ua / voltage_power_supply * voltage_exchange; - duty_b = Ub / voltage_power_supply * voltage_exchange; - duty_c = Uc / voltage_power_supply * voltage_exchange; +// // 设置PWM输出 +// float voltage_exchange = limit_voltage / v_safe; +// duty_a = Ua / voltage_power_supply * voltage_exchange; +// duty_b = Ub / voltage_power_supply * voltage_exchange; +// duty_c = Uc / voltage_power_supply * voltage_exchange; -<<<<<<< HEAD -======= - 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); -} +// Set_3Pwm(duty_a, duty_b, duty_c); +// } /** * @brief 使用SVPWM算法设置电机输出 @@ -382,11 +248,7 @@ inline void FOC_Controller::setSVPWM_PhaseVoltage(float Uq, float Ud) { * */ void FOC_Controller::shut_down() { -<<<<<<< HEAD drv_stop(); -======= - is_shut_down = 1; ->>>>>>> c9934849c58e5b5daedd91d8bfde3d56aa02f83f motor_sleep(); is_shut_down = true; } @@ -450,11 +312,7 @@ void FOC_Controller::update_power(float voltage) { voltage_error = false; voltage_power_supply = voltage; v_half = voltage * 0.5f; -<<<<<<< HEAD v_safe = voltage * 1.1f; -======= - v_safe = voltage * 1.05f; ->>>>>>> c9934849c58e5b5daedd91d8bfde3d56aa02f83f } else { voltage_error = true; } @@ -469,7 +327,6 @@ void FOC_Controller::update_power(float voltage) { float FOC_Controller::velocity_open_loop() { // 通过时间间隔和目标速度计算需要转动的机械角度，存储在shaft_angle中，在此之前，还需要对轴角度归一化 shaft_angle = fmod(shaft_angle + target_velocity * REDUCTION_RATIO * 1.0 / -<<<<<<< HEAD MOTOR_FREQUENCY / 2, 360.0f); // 获取电机轴角度 @@ -481,19 +338,6 @@ float FOC_Controller::velocity_open_loop() { // 因为在开环控制中，Ud始终为0，只控制Uq const float Uq = voltage_power_supply / 2.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; } @@ -513,84 +357,6 @@ float FOC_Controller::angle_control() { sampler.get_current('W')); // 设置相电压 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; } diff --git a/Robot_V2/Algorithm/FOC.h b/Robot_V2/Algorithm/FOC.h index 7b3cc2c..601692f 100644 --- a/Robot_V2/Algorithm/FOC.h +++ b/Robot_V2/Algorithm/FOC.h @@ -62,8 +62,7 @@ class FOC_Controller { float drv_hold(void); void shut_down_thermal(void); // 过热关机 void resume_from_thermal(void); // 过热恢复 - - void setSVPWM_PhaseVoltage(float Uq, float Ud); + // 此处默认值不作数 PIDController vel_loop = 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 startup_ramp_val = 0.0f; // 软启动系数 float startup_ramp_step = 0.01f; // 启动斜坡步长，可调节以控制启动速度 - void setPhaseVoltage(float Uq, float Ud); void setSVPWM_PhaseVoltage(float Uq, float Ud); float cal_Iq_Id(float current_a, float current_b, float current_c); }; diff --git a/Robot_V2/Algorithm/pid.cpp b/Robot_V2/Algorithm/pid.cpp index 25af6a3..9c28762 100644 --- a/Robot_V2/Algorithm/pid.cpp +++ b/Robot_V2/Algorithm/pid.cpp @@ -6,7 +6,6 @@ #include #include - extern CAN_transmiter CAN0_transmiter; PIDController::PIDController(float P, float I, float D, float ramp, float limit) : 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参数 * @return void */ -void PIDController::set_P_percent(float pp) { this->P = pp; } \ No newline at end of file +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; +} \ No newline at end of file diff --git a/Robot_V2/device/ADC.cpp b/Robot_V2/device/ADC.cpp index 907fb7a..da04452 100644 --- a/Robot_V2/device/ADC.cpp +++ b/Robot_V2/device/ADC.cpp @@ -15,7 +15,7 @@ constexpr float VCC_FACTOR = 11.0f; constexpr float CURRENT_FACTOR = 1.0f / 50.0f * 100.0f; // 50倍放大，100倍电流采样电阻, 单位mA extern CAN_transmiter CAN0_transmiter; -extern FOC_Controlroller FOC; +extern FOC_Controller FOC; sampler sampler; // 采样器初始化 u16 adc_value[6] = {0}; // 显式初始化数组元素为0 /*! diff --git a/Robot_V2/device/Can.cpp b/Robot_V2/device/Can.cpp index 9367cf9..9f6a848 100644 --- a/Robot_V2/device/Can.cpp +++ b/Robot_V2/device/Can.cpp @@ -19,9 +19,7 @@ #include constexpr u16 CANX = CAN0; - -CAN_transmiter CAN0_transmiter; // CAN初始化 -extern FOC_Controlroller FOC; +extern FOC_Controller FOC; extern sampler sampler; extern angle_sensor KTH7823; extern EncoderAngleKalman Angle_kalman; // 角度滤波 @@ -575,37 +573,47 @@ void CAN_transmiter::communication_check() { // } 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 = - float(unpack_bytes_to_u16(receive_message.rx_data, 1)) / 1000; - // 目标电流&目标速度 - FOC.limit_velocity = - unpack_bytes_to_float(receive_message.rx_data, 3).f; - FOC.target_current = FOC.limit_velocity; - // 目标角度 - target_angle_union.f = - unpack_bytes_to_float(receive_message.rx_data, 7).f; - FOC.target_angle = target_angle_union.f; + float(unpack_bytes_to_u16(current_message.rx_data, 1)) / 1000; + // 目标电流 + target_current_union.f = + unpack_bytes_to_float(current_message.rx_data, 3).f; + FOC.current_loop.set_limit(target_current_union.f); + FOC.vel_inMITloop.set_limit(target_current_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_current = target_current_union.f; // 解析PID参数 float voltage = FOC.limit_voltage; - FOC.current_loop.set_P_percent(current_loop_p * P_config / voltage); - P_config = unpack_bytes_to_u16(receive_message.rx_data, 17); - FOC.vel_loop.set_P_percent(vel_loop_p * P_config / voltage); - P_config = unpack_bytes_to_u16(receive_message.rx_data, 19); - FOC.angle_loop.set_P_percent(angle_loop_p * P_config / voltage); - P_config = unpack_bytes_to_u16(receive_message.rx_data, 21); - FOC.vel_inVelAngloop.set_P_percent(vel_angloop_p * P_config / - voltage); - } else if (receive_message.rx_efid == CANFD_RESET_ID) { - u8 first_byte = receive_message.rx_data[0]; - u8 eigth_byte = receive_message.rx_data[7]; + // PID参数 + u16 P_config = unpack_bytes_to_u16(current_message.rx_data, 15); + FOC.cur_loop_p = DEFAULT_current_loop_p * P_config / voltage; + FOC.cur_MIT_p = DEFAULT_current_MIT_p * P_config / voltage; + + P_config = unpack_bytes_to_u16(current_message.rx_data, 17); + FOC.vel_loop_p = DEFAULT_vel_loop_p * P_config / voltage; + FOC.vel_angloop_p = DEFAULT_vel_angloop_p * P_config; + FOC.vel_MIT_p = DEFAULT_vel_MIT_p * P_config; + + 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) { motor_reset(); diff --git a/Robot_V2/device/Timer.cpp b/Robot_V2/device/Timer.cpp index 14ac98d..c5377d5 100644 --- a/Robot_V2/device/Timer.cpp +++ b/Robot_V2/device/Timer.cpp @@ -11,7 +11,7 @@ volatile u32 total_us100_counter = 0; // 总计数器，每 100 微秒增加 1 extern u16 adc_value[7]; -extern FOC_Controlroller FOC; +extern FOC_Controller FOC; extern TaskScheduler scheduler; extern CAN_transmiter CAN0_transmiter; // 添加系统运行时间计数器 @@ -131,79 +131,49 @@ void TIMER2_IRQHandler(void) { } timer_interrupt_flag_clear(TIMER2, TIMER_INT_UP); - // 每50us执行一次的操作 - 电机控制 - scheduler.taskFlags |= (1 << TASK_MOTOR_CONTROL); - scheduler.taskFlags |= - (1 << TASK_ANGLE_UPDATE) | (1 << TASK_VELOCITY_UPDATE); + // 固定任务：每 50us 执行电机控制 + scheduler.taskFlags |= (1u << TASK_ANGLE_UPDATE) | + (1u << TASK_VELOCITY_UPDATE) | + (1u << TASK_MOTOR_CONTROL); - // 使用标志位控制，每100us执行一次其他任务 - static bool isEvenCall = false; - isEvenCall = !isEvenCall; - - if (isEvenCall) { - // 每100us执行一次的操作 - 保持原有频率 - - 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++; - if (total_us100_counter > 9) { - total_us100_counter = 0; - // 可以添加每1ms执行的代码，无负载均衡 - static u8 ms1_counter = 0; - - switch (ms1_counter) { // 每10ms执行的代码，负载均衡 - case 0: { - scheduler.taskFlags |= (1 << TASK_THERMAL_CONTROL); - break; - } - case 1: { - scheduler.taskFlags |= (1 << TASK_MAGNETIC_CHECK); - break; - } - case 2: { - break; - } - } - ms1_counter++; - if (ms1_counter > 9) { - ms1_counter = 0; - // 可以添加每10ms执行的代码，无负载均衡 - 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执行的代码 - } - } - } - } - } - } + // 每 100us 执行一次的任务（使用位运算替代布尔变量） + static u32 call_counter = 0; + call_counter++; + + if ((call_counter & 1u) == 0u) { // 偶数次调用（每 100us） + // 使用查找表设置 1ms 周期内的任务 + scheduler.taskFlags |= task_schedule_100us[total_us100_counter]; + + total_us100_counter++; + if (total_us100_counter >= 10u) { + total_us100_counter = 0; + + // 每 1ms 执行的任务 + static u32 ms1_counter = 0; + ms1_counter++; + if (ms1_counter >= 10u) { + ms1_counter = 0; + + // 每 10ms 执行的任务 + static u32 ms10_counter = 0; + ms10_counter++; + if (ms10_counter >= 10u) { + ms10_counter = 0; + + // 每 100ms 执行的任务 + static u32 ms100_counter = 0; + ms100_counter++; + if (ms100_counter >= 10u) { + 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]; + } + } } /** diff --git a/Robot_V2/main.cpp b/Robot_V2/main.cpp index ab0a824..c78d2d6 100644 --- a/Robot_V2/main.cpp +++ b/Robot_V2/main.cpp @@ -16,7 +16,7 @@ extern angle_sensor KTH7823; // 角度传感器 extern EncoderVelocityKalman Vel_kalman; // 速度滤波 extern EncoderAngleKalman Angle_kalman; // 角度滤波 extern ThermalController thermalController; // 热量控制器 -extern FOC_Controller FOC; // FOC控制 +extern FOC_Controller FOC; // FOC控制 extern FlashEEPROM flashEEP; // EEPROM类 extern VersionManager Version; @@ -95,21 +95,35 @@ void do_task(u32 currentTasks) { if (currentTasks & (1 << TASK_MOTOR_CONTROL)) { // 电机控制任务(最高优先级) motor_runtime_50us++; - // 函数指针数组，包含各种控制模式对应的函数 - static FOCFunctionPtr control_functions[] = { - &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: 速度开环控制 - }; + // 静态变量用于跟踪上一次的控制模式 + static int last_control_mode = -1; // 边界检查，防止数组越界 - if (FOC.control_mode >= 0 && FOC.control_mode < 5) { // 更新为7个控制模式 - // 调用对应的控制函数 - (FOC.*control_functions[FOC.control_mode])(); + 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])(); + } else { + // 调用对应的控制函数 + FOC.drv_stop(); + } } } if (currentTasks & (1 << TASK_MCU_TEMP_UPDATE)) { // 更新mcu温度传感器值 diff --git a/Robot_V2/sys/config.h b/Robot_V2/sys/config.h index 8ef0f83..8ea015c 100644 --- a/Robot_V2/sys/config.h +++ b/Robot_V2/sys/config.h @@ -1,5 +1,6 @@ #ifndef __CONFIG_H #define __CONFIG_H +#include "gd32c10x_eval.h" #include // 如果使用CMSIS DSP库，需要定义ARM_MATH_CM4 @@ -54,7 +55,7 @@ constexpr u32 OTA_AVAL = 0x00000000; // 全零表示可用 constexpr u8 PP = 11; constexpr float DIR = -1.0f; constexpr u16 REDUCTION_RATIO = 25; -constexpr float CURRENT_OFFSET = -3.31f/2.0f; // 当前电流传感器的零点偏移 +constexpr float CURRENT_OFFSET = -3.31f / 2.0f; // 当前电流传感器的零点偏移 constexpr float HOLD_PWM_DUTY = 0.1f; constexpr u16 ANGLESENS_OFFSET = +15; constexpr float KE_FACTOR = 0.0f; @@ -87,17 +88,49 @@ constexpr float CURRENT_KALMAN_PROCESS_NOISE_Q = 10.0f; // 降低过程噪声 constexpr float CURRENT_KALMAN_MEASUREMENT_NOISE_R = 5e-5f; // 增大测量噪声 // PID配置 -constexpr float current_loop_p = 6e-2f / 100 * 12; // 电流P -constexpr float vel_loop_p = 0.01f / 100 * 12; // 速度P -constexpr float angle_loop_p = 20.0f / 100 * 12; // 角度P -constexpr float vel_angloop_p = 0.12f / 100 * 12; // 速度 速度-角度P -constexpr float ang_angloop_p = 100.0f / 100 * 12; // 角度 速度-角度P +constexpr float DEFAULT_current_loop_p = 0.1f / 100 * 12; // 电流P +constexpr float current_loop_Ki = 0.55f; // 电流P +constexpr float current_loop_Kd = 3e-4f; // 电流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_U32 = sizeof(uint32_t); constexpr float ADC_MAX = 65536.0f; 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 DEG2RAD = 1.0f / 180.0f * PI; constexpr float RAD2DEG = 180.0f / PI; // 修正RAD2DEG定义，应该是180/π @@ -149,8 +182,12 @@ constexpr float ANGLESENS_FACTOR = 1.0f / ADC_MAX * 360.0f; // TIMER2-任务调度 constexpr int TIMER2_PRESCALER = 119; // 120MHz / 120 = 1MHz -constexpr int TIMER2_PERIOD = 49; // 1MHz / 50 = 50us -constexpr float TIMER2_FREQUENCY = 1 / 5e-5; +constexpr int TIMER2_PERIOD = 49; // 1 / 1MHz * 50 = 50us +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 constexpr int TIMER1_PRESCALER = 0; // 不分频，计数器时钟为120MHz