[STM32+HAL]DengFOC移植之闭环位置控制

一、源码来源

DengFOC官方文档

二、HAL库配置

1、开启硬件IIC低速模式

低速更稳定

2、PWM波开启

三、keil填写代码

1、AS5600读取编码器数值

#include "AS5600.h"
#include "math.h"
 
 
float angle_prev=0;
int full_rotations=0; 		// full rotation tracking;
float angle_d;				//GetAngle_Without_Track()的返回值
float angle_cd;				//GetAngle()的返回值
 
 
//IIC读多字节
void AS5600_Read_Reg(uint16_t reg, uint8_t* buf, uint8_t len)
{
	HAL_I2C_Mem_Read(&hi2c1, AS5600_ADDRESS, reg, I2C_MEMADD_SIZE_8BIT, buf, len, 100);
}
 
 
 
//得到弧度制的角度，范围在0-6.28
float GetAngle_Without_Track(void)
{
	int16_t in_angle;
	uint8_t temp[DATA_SIZE]={0};
	AS5600_Read_Reg( Angle_Hight_Register_Addr, temp, DATA_SIZE);
	in_angle = ((int16_t)temp[0] <<8) | (temp[1]);
 
	angle_d = (float)in_angle * (2.0f*PI) / 4096;
//angle_d为弧度制，范围在0-6.28
	return angle_d;
}
 
 
 
//得到弧度制的带圈数角度
float GetAngle(void)
{
    float val = angle_d;
    float d_angle = val - angle_prev;
    //计算旋转的总圈数
    //通过判断角度变化是否大于80%的一圈(0.8f*6.28318530718f)来判断是否发生了溢出，如果发生了，则将full_rotations增加1（如果d_angle小于0）或减少1（如果d_angle大于0）。
    if(fabs(d_angle) > (0.8f*2.0f*PI) ) full_rotations += ( d_angle > 0 ) ? -1 : 1;
    angle_prev = val;
 
	angle_cd = full_rotations * (2.0f*PI) + angle_prev;
	return angle_cd;
}
 
 
 
void Track(void)
{
	GetAngle_Without_Track();
	GetAngle();
}

2、闭环FOC控制

#include "AS5600.h"
#include "FOC1.h"
#include <math.h>
 
 
#define PWMA TIM1 -> CCR1
#define PWMB TIM1 -> CCR2
#define PWMC TIM1 -> CCR3
#define CNT  TIM1 -> ARR-1
 
 
float voltage_limit=12.6;
float voltage_power_supply=12.6;
float shaft_angle=0,open_loop_timestamp=0;
float zero_electric_angle=0,Ualpha,Ubeta=0,Ua=0,Ub=0,Uc=0,dc_a=0,dc_b=0,dc_c=0;
int PP=7,DIR=-1;
 
 
float _electricalAngle(void){
  return  _normalizeAngle((float)(DIR *  PP) * GetAngle_Without_Track()-zero_electric_angle);
}
 
 
// 归一化角度到 [0,2PI]
float _normalizeAngle(float angle){
  float a = fmod(angle, 2*PI);   //取余运算可以用于归一化，列出特殊值例子算便知
  return a >= 0 ? a : (a + 2*PI);
}
 
 
// 设置PWM到控制器输出
void setPwm(float Ua, float Ub, float Uc) {
 
  // 限制上限
  Ua = _constrain(Ua, 0.0f, voltage_limit);
  Ub = _constrain(Ub, 0.0f, voltage_limit);
  Uc = _constrain(Uc, 0.0f, voltage_limit);
  // 计算占空比
  // 限制占空比从0到1
  dc_a = _constrain(Ua / voltage_power_supply, 0.0f , 1.0f );
  dc_b = _constrain(Ub / voltage_power_supply, 0.0f , 1.0f );
  dc_c = _constrain(Uc / voltage_power_supply, 0.0f , 1.0f );
 
  //写入PWM到PWM 0 1 2 通道
	PWMA = dc_a*5599;
	PWMB = dc_b*5599;
	PWMC = dc_c*5599;
}
 
void setPhaseVoltage(float Uq,float Ud, float angle_el) {
  angle_el = _normalizeAngle(angle_el);
  // 帕克逆变换
  Ualpha =  -Uq*sin(angle_el);
  Ubeta =   Uq*cos(angle_el);
 
  // 克拉克逆变换
  Ua = Ualpha + voltage_power_supply/2;
  Ub = (sqrt(3)*Ubeta-Ualpha)/2 + voltage_power_supply/2;
  Uc = (-Ualpha-sqrt(3)*Ubeta)/2 + voltage_power_supply/2;
  setPwm(Ua,Ub,Uc);
}
 
 
 
//初始化FOC，校准零点
void FOC_Init(void)
{
	setPhaseVoltage(3, 0,_3PI_2);
	HAL_Delay(1000);
	zero_electric_angle=_electricalAngle();
	setPhaseVoltage(0, 0,_3PI_2);
}

3、main.c

/* USER CODE BEGIN PV */
extern float voltage_limit;
extern float voltage_power_supply;
extern float shaft_angle,open_loop_timestamp;
extern float zero_electric_angle,Ualpha,Ubeta,Ua,Ub,Uc,dc_a,dc_b,dc_c;
extern int PP,DIR;
 
float motor_target = 4;
/* USER CODE END PV */  
 
 
/* USER CODE BEGIN 2 */
	printf("Hello World\r\n");
	HAL_Delay(500);
 
	HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_1);
	HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_2);
	HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_3);
	
	FOC_Init();
	HAL_TIM_Base_Start_IT(&htim2);
  /* USER CODE END 2 */
 
  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
		float Sensor_Angle=GetAngle();
		float Kp=0.133;
		setPhaseVoltage(_constrain(Kp*(motor_target-DIR*Sensor_Angle)*180/PI,-6,6),0,_electricalAngle());
    /* USER CODE END WHILE */
 
    /* USER CODE BEGIN 3 */
  }
  /* USER CODE END 3 */