C语言 FOC 位置控制循环函数解析及注释

void FOC_position_control_loop(float target_angle) { '目标角度归一化' target_angle = _normalizeAngle(target_angle); '当前角度归一化' float now_angle = _normalizeAngle(FOC_get_mechanical_angle() - zero_mechanical_angle); //float now_angle = FOC_get_mechanical_angle();

'计算角度误差'
float angle_error = target_angle - now_angle;
'如果角度误差小于负π，则目标角度加上2π'
if (angle_error < -_PI) target_angle += _2PI;
'如果角度误差大于π，则目标角度减去2π'
else if (angle_error > _PI) target_angle -= _2PI;

'计算目标速度'
float target_velocity = pid_get_u(&pid_position, target_angle, now_angle);
'当前速度经过低通滤波器滤波'
float now_velocity = FOC_low_pass_filter(&lpf_velocity, FOC_get_velocity());
'计算电压量'
float Uq = pid_get_u(&pid_velocity, target_velocity, now_velocity);
'计算电气角度'
float electrical_angle = FOC_electrical_angle();
'进行SVPWM控制'
FOC_SVPWM(Uq, 0, electrical_angle);

//printf("%.2f,%.2f,%.2f,%.2f\n", target_angle, now_angle, target_velocity, now_velocity);

}