cbb/Motor_v2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
bdf03517b31de208ce811fa5cc0daea1ba22f698
Motor_v2/Robot_V2/Algorithm/FOC.cpp
T
cbb bdf03517b3 feat(CAN): 添加静默模式支持并优化节点ID处理 
- 添加静默模式功能，在接收到base_id=0x0000消息时停止发送10秒
- 修正节点ID加载逻辑，区分有效节点ID(0-65534)和Flash擦除状态(0xFFFF)
- 添加静默模式相关的状态检查和时间管理功能
- 在CAN通信检查中集成静默模式处理流程

fix(FOC): 修复速度控制中的方向参数应用
- 在velocity_control、angle_control_with_velocity_limit和MIT_Mode函数中
 正确应用DIR参数调整速度误差的符号
- 添加详细注释说明DIR参数对速度误差计算的影响

refactor(systick): 添加毫秒级系统时间功能
- 实现get_systick_ms()函数提供毫秒级系统时间
- 添加SysTick中断处理程序更新系统时间计数器

chore(init): 调整初始化顺序优化系统启动流程
- 将EEPROM初始化移至CAN初始化之前，确保正确的依赖关系
2026-03-19 23:49:08 +08:00

492 lines
14 KiB
C++
Raw Blame History

#include "FOC.h"
#include "Can.h"
#include "Filter.h"
#include "Timer.h"
#include "adc.h"
#include "ang_sensor.h"
#include "config.h"
#include "motor.h"
#include "pid.h"
#include "systick.h"
#include <arm_math.h>
#include <cmath>
#include <gd32c10x.h>
FOC_Controller FOC; // FOC控制初始化
extern sampler sampler;
extern angle_sensor KTH7823;
extern CAN_transmiter CAN0_transmiter;
extern EncoderVelocityKalman Vel_kalman; // 速度滤波
extern CurrentKalman Current_kalman;
// 初始变量及函数定义
#define _constrain(amt, low, high) \
((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt)))
#define constrain(value, min, max, target_min, target_max) \
((value - min) / (max - min) * (target_max - target_min))
// 校准步骤（代码模板）
void calibrate_zero_position(void) {
// Set_3Pwm(.25, 0, 0); // 起劲
// delay_1ms(1000);
// KTH7823.update();
// // 改进校准算法：获取当前传感器角度并计算补偿值
// float current_sensor_angle = KTH7823.getcurrent_sensorangle();
// float electrical_angle = fmodf(current_sensor_angle * PP, 360.0f); //
// 计算当前电角度
// // 计算需要的补偿值，使得当前位置对应0°电角度
// KTH7823.calibration_angle = -electrical_angle;
// // 对补偿角进行归一化处理，确保在[-180, 180]范围内
// KTH7823.calibration_angle = fmodf(KTH7823.calibration_angle + 180.0f,
// 360.0f); if (KTH7823.calibration_angle < 0) { KTH7823.calibration_angle
// += 360.0f;
// }
// KTH7823.calibration_angle -= 180.0f;
// Set_3Pwm(0, 0, 0); // 停止
}
// 通过Ia,Ib,Ic计算Iq,Id(目前仅输出Iq)
inline float FOC_Controller::cal_Iq_Id(float current_a, float current_b,
float current_c) {
// 计算电气角度
float angle_el = fmod(shaft_angle * PP - KTH7823.calibration_angle, 360.0f);
// Clarke变换 (使用三相电流)
float I_alpha, I_beta;
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;
// Park变换
float sin_el, cos_el;
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;
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;
}
/**
* @brief 电机控制类初始化函数
*
* @return u8 0表示初始化成功，1表示初始化失败
*/
u8 FOC_Controller::init() {
motor_init(); // 电机驱动初始化
FOC.cur_loop_p = DEFAULT_current_loop_p * 100 / limit_voltage;
FOC.cur_MIT_p = DEFAULT_current_MIT_p * 100 / limit_voltage;
FOC.vel_loop_p = DEFAULT_vel_loop_p * 100 / limit_voltage;
FOC.vel_angloop_p = DEFAULT_vel_angloop_p * 100;
FOC.vel_MIT_p = DEFAULT_vel_MIT_p * 100;
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;
return 0;
}
/**
* @brief 电机控制类构造函数
*
*/
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);
// // 帕克逆变换 (直接计算，避免函数调用开销)
// 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;
// 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;
// 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 = shaft_angle * PP + KTH7823.calibration_angle;
// 对电角度进行归一化，确保在[0, 360)范围内
angle_el = fmodf(angle_el, 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.0f)
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.5f;
duty_b = 0.5f;
duty_c = 0.5f;
break;
}
// 设置PWM输出
Set_3Pwm(duty_a, duty_b, duty_c);
}
/**
* @brief 电机关机（通用关机，不带原因标记）
*
*/
void FOC_Controller::shut_down() {
drv_stop();
motor_sleep();
is_shut_down = true;
}
/**
* @brief 过热关机
*
*/
void FOC_Controller::shut_down_thermal() {
drv_stop();
motor_sleep();
is_shut_down = true;
thermal_shutdown = true;
}
/**
* @brief 过热恢复
*
*/
void FOC_Controller::resume_from_thermal() {
thermal_shutdown = false;
if (!voltage_error && !over_current) {
is_shut_down = false;
motor_resume();
}
}
/**
* @brief 占空比置0
*
*/
void FOC_Controller::drv_stop() { Set_3Pwm(0.0f, 0.0f, 0.0f); }
/**
* @brief 电机锁止
*/
float FOC_Controller::drv_hold() {
float hold_pwm_duty = HOLD_PWM_DUTY;
if (HOLD_PWM_DUTY > 0.95f) {
hold_pwm_duty = 0.95f;
} else if (HOLD_PWM_DUTY < 0.0f) {
hold_pwm_duty = 0.0f;
}
Set_3Pwm(hold_pwm_duty, 0, 0);
return hold_pwm_duty;
}
/**
* @brief 电机恢复
*
*/
void FOC_Controller::resume() {
over_current = false;
voltage_error = false;
is_shut_down = false;
motor_resume();
}
/**
* @brief 更新电源电压
*
*/
void FOC_Controller::update_power(float voltage) {
if (voltage < 36.0f and voltage > 8.0f) {
voltage_error = false;
voltage_power_supply = voltage;
v_half = voltage * 0.5f;
v_safe = voltage * 1.1f;
} else {
voltage_error = true;
}
}
/**
* @brief 速度开环控制
*
* @param target_velocity: 目标速度
* @return float: q轴电压
*/
float FOC_Controller::velocity_open_loop() {
// 通过时间间隔和目标速度计算需要转动的机械角度，存储在shaft_angle中，在此之前，还需要对轴角度归一化
shaft_angle = fmod(shaft_angle + target_velocity * REDUCTION_RATIO * 1.0 /
MOTOR_FREQUENCY / 2,
360.0f);
// 获取电机轴角度
float shaft_angle = KTH7823.getcurrent_sensorangle();
CAN0_transmiter.testfloat_union.f = shaft_angle;
rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
sampler.get_current('W'));
// 因为在开环控制中，Ud始终为0，只控制Uq
const float Uq = voltage_power_supply / 2.0f;
setSVPWM_PhaseVoltage(Uq, 0.0f);
return limit_voltage;
}
/**
* @brief 角度闭环控制
*
* @param target_angle: 目标角度
* @return float: 当前角度
*/
float FOC_Controller::angle_control() {
// PID 控制计算
float Uq = angle_loop(rt_angle - target_angle);
// 获取电机轴角度
shaft_angle = KTH7823.getcurrent_sensorangle();
rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
sampler.get_current('W'));
// 设置相电压
setSVPWM_PhaseVoltage(Uq, 0.0f);
return shaft_angle;
}
/**
* @brief 速度闭环控制
*
* @param target_velocity: 目标速度
* @return float: 当前速度
*/
float FOC_Controller::velocity_control() {
// PID 控制计算，使用 DIR 参数调整速度误差的符号
// DIR = -1.0f 时：速度误差 = -(target_velocity - rt_velocity) = rt_velocity - target_velocity
// DIR = 1.0f 时：速度误差 = target_velocity - rt_velocity
float Uq = vel_loop((target_velocity - rt_velocity) * DIR * REDUCTION_RATIO);
CAN0_transmiter.testfloat_union.f = Uq;
// 获取电机轴角度
shaft_angle = KTH7823.getcurrent_sensorangle();
// 设置相电压
setSVPWM_PhaseVoltage(Uq, 0.0f);
rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
sampler.get_current('W'));
return rt_velocity;
}
float FOC_Controller::current_control() {
// PID 控制计算
float Iq = rt_current =
cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
sampler.get_current('W'));
float Uq = current_loop(target_current - Iq);
if (Uq > limit_voltage) {
Uq = limit_voltage;
} else if (Uq < -limit_voltage) {
Uq = -limit_voltage;
}
// 获取电机轴角度
shaft_angle = KTH7823.getcurrent_sensorangle();
CAN0_transmiter.testfloat_union.f = shaft_angle;
// 设置相电压
setSVPWM_PhaseVoltage(Uq, 0.0f);
return 1;
}
/**
* @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;
ang_inVelAngloop.set_P(ang_angloop_p);
// 使用角度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(target_velocity - rt_velocity)* DIR;
CAN0_transmiter.testfloat_union.f = Uq;
// // 实现力度控制：如果电流超过目标电流，则限制输出
// if (abs(rt_current) > abs(target_current)) {
// // 当实际电流超过目标电流时，减小输出以限制电流
// // 可以采用比例限制或直接限制输出电压的方式
// float current_ratio = abs(target_current) / abs(rt_current);
// if (current_ratio < 1.0f) {
// Uq *= current_ratio; // 按比例缩小输出电压
// }
// }
// 获取电机轴角度
shaft_angle = KTH7823.getcurrent_sensorangle();
// 获取当前实际电流值
rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
sampler.get_current('W'));
// 设置相电压
setSVPWM_PhaseVoltage(Uq, 0.0f);
return shaft_angle;
}
/**
* @brief MIT控制
*
* @param target_angle: 目标角度
* @param max_velocity: 最大速度限制 (单位: rad/s)
* @return float: 当前角度
*/
float FOC_Controller::MIT_Mode() {
// 第一层：角度环（外环）
// 根据目标角度和实际角度的差值，通过角度环PID控制器计算目标速度
float angle_error = target_angle - rt_angle;
ang_inMITloop.set_P(ang_MIT_p);
float target_velocity =
ang_inMITloop(angle_error); // 使用angle_loop作为角度到速度的映射
// 第二层：速度环（中环）
// 根据目标速度和实际速度的差值，通过速度环PID控制器计算目标电流
float velocity_error = (target_velocity - rt_velocity)* DIR;
vel_inMITloop.set_P(vel_MIT_p);
float target_current = vel_inMITloop(velocity_error);
// 第三层：电流环（内环）
// 计算当前实际电流
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;
current_inMITloop.set_P(cur_MIT_p);
float Uq = current_inMITloop(current_error);
// 获取电机轴角度
shaft_angle = KTH7823.getcurrent_sensorangle();
// 设置相电压
setSVPWM_PhaseVoltage(Uq, 0.0f);
return shaft_angle;
}
Reference in New Issue View Git Blame Copy Permalink