一、源码来源
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.28return 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-1float 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到1dc_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 */
