FOC算法实现