STM32上跑SimpleFOC，电流环、速度环、位置环、棘轮软硬件全开源

引入

我之前写过不少SVPWM、FOC的介绍文章，比如：
SVPWM算法原理及详解
从电机本质到park变换再到SVPWM，SVPWM代码实现
电机FOC算法的解释
FOC和SVPWM的C语言代码实现

simple foc可以看成是他们的简化版本。本来simple foc是跑在arduino上的，为了方便网上有人把它移植到了STM32上，我这里也只是拿过来介绍并讲解下，方便大家使用。
simple foc学习可以去看灯哥的视频，一行一行代码教你写出来。
灯哥手把手教你写FOC算法 系列课程

硬件、软件例程下载地址：
链接: https://pan.baidu.com/s/1OGqoYWYTxoRtALSN4BOnpA 提取码: 24kv
包含4个例程和硬件板子：

硬件

硬件使用立创EDA：


电路很简单，主控STM32F103C8T6，电机驱动DRV8313PWPR（最大电压60V，最大电流2.5A），
电流采集芯片INA199A1，12位磁编AS5600。

把板子做好后，需要如下配件来组装：
1个3205b无刷电机 带径向磁环（最好买去除限位的，这样可以一直转，不然只能转1圈。电机要带径向磁环配合磁编，不然就需要自己贴磁钢。参考购买链接）
3根8mm长M2尼龙铜柱
3个M2尼龙螺丝

除了以上 3205B，还建议选用以下较为常见的云台电机：

电流环

框图如下，通过电流传感器读取电流后做clarke、park变换，然后做PID

框图如下，通过电流传感器读取电流后做clarke、park变换，然后做PID，最后输出给SVPWM。

		//电流闭环	
		shaft_velocity = shaftVelocity();      //电机转速
		shaft_angle = shaftAngle();            //转子电角度
		electrical_angle = electricalAngle();  //转子电角度减去零位
		current_ADC.a=-((float)Get_Adc_Average(ADC_Channel_3,1)*(3.3/4096)-1.65)*0.4; //电机相电流 1÷0.05÷50=0.4
		current_ADC.b=-((float)Get_Adc_Average(ADC_Channel_4,1)*(3.3/4096)-1.65)*0.4;	
			
		current_cal = getFOCCurrents(_normalizeAngle(electrical_angle));  //相电流做clarke、park变换
		
		// filter values
//		current.q = LPFoperator(&LPF_current_q,current.q);  //低通滤波
//		current.d = LPFoperator(&LPF_current_d,current.d);
		// calculate the phase voltages
		
		voltage.d = PID_current_my_d( current_tar.d-current_cal.d);  //当前电流和目前电流做PID计算
		voltage.q = PID_current_my(current_tar.q- current_cal.q); 
		
		setPhaseVoltage(voltage.q, voltage.d , electrical_angle);    //SVPWM
		//电流闭环	

程序下载后，电机就会转动。

速度环

目标速度和当前速度做PID，然后输出给SVPWM。

		//速度环********************************			
		shaft_velocity = shaftVelocity();
			
		// calculate the torque command
		current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if current/foc_current torque control
		// if torque controlled through voltage control 
		voltage.q = current_sp;  // use voltage if phase-resistance not provided
		voltage.d = 0;

		shaft_angle = shaftAngle();// shaft angle
		electrical_angle = electricalAngle();// electrical angle - need shaftAngle to be called first
		setPhaseVoltage(voltage.q, voltage.d, electrical_angle);
		//速度环	

程序下载后，电机就会以shaft_velocity_sp 速度转动。

位置环

目标位置和当前位置做PID，然后输出给SVPWM。

		//位置环********************************
		//shaft_angle_sp=2;//在这里设置
		shaft_angle = shaftAngle();

		// calculate the torque command
		current_sp = PID_angle(shaft_angle_sp - shaft_angle); // if current/foc_current torque control
		// if torque controlled through voltage control 
		voltage.q = current_sp;  // use voltage if phase-resistance not provided
		voltage.d = 0;

		electrical_angle = electricalAngle();// electrical angle - need shaftAngle to be called first
		setPhaseVoltage(voltage.q, voltage.d, electrical_angle);
		//位置环

程序下载后，电机会锁定在shaft_angle_sp 位置，shaft_angle_sp会隔1s自增。

棘轮旋钮

实现棘轮旋钮的效果

		shaft_angle = shaftAngle();// shaft angle	
		electrical_angle = electricalAngle();// electrical angle - need shaftAngle to be called first
		//棘轮旋钮：给不同位置施加不同的电流（转矩）
		//棘轮模式，中点无输出0，边界给力矩1,2，超过了阈值切换中点3,4
		if (shaft_angle >= ratchet_center_angle - ratchet_center_dead_angle && 
        shaft_angle <= ratchet_center_angle + ratchet_center_dead_angle) 
		{
			setPhaseVoltage(0,0, electrical_angle);
			flag=0;
		}
		else if (shaft_angle <= ratchet_center_angle-ratchet_boundary_angle) 
		{
			ratchet_center_angle=shaft_angle-ratchet_step_angle;
		}
		else if (shaft_angle >= ratchet_center_angle+ratchet_boundary_angle) 
		{
				ratchet_center_angle=shaft_angle+ratchet_step_angle;
		}
		else
		{
			if (shaft_angle <= ratchet_center_angle)
			{
				force=(shaft_angle-ratchet_center_angle+ratchet_center_dead_angle)*force_MAX/ratchet_boundary_angle*4;
				force=-force;
				if(force>force_MAX)force=force_MAX;
					setPhaseVoltage(force,0, electrical_angle);//为正的力
				flag=1;
			}
			if (shaft_angle >= ratchet_center_angle)
			{
				force=(shaft_angle-ratchet_center_angle-ratchet_center_dead_angle)*force_MAX/ratchet_boundary_angle*4;
				force=-force;
				if(force<-force_MAX)force=-force_MAX;
				setPhaseVoltage(force,0, electrical_angle);//为负的力
				flag=2;
			}
		}

程序下载后，扭动电机会有棘轮的手感。