#include "DengFOC.h"
int Sensor_DIR=
-1; //传感器方向
int Motor_PP=7; //电机极对数
void setup
() {
Serial.begin
(115200
);
DFOC_Vbus
(12.6
); //设定驱动器供电电压
DFOC_alignSensor
(Motor_PP,Sensor_DIR
);
}
void loop
()
{
//设置速度环PID
DFOC_M0_SET_VEL_PID
(0.005,0.00,0,0
);
//设置速度
DFOC_M0_setVelocity
(serial_motor_target
());
//接收串口
serialReceiveUserCommand
();
}
#include "AS5600.h"
#include "Wire.h"
#include <Arduino.h>
#define _2PI 6.28318530718f
// AS5600 相关
double Sensor_AS5600::getSensorAngle
() {
uint8_t angle_reg_msb = 0x0C;
byte readArray[2];
uint16_t readValue = 0;
wire
->beginTransmission
(0x36
);
wire
->write
(angle_reg_msb
);
wire
->endTransmission
(false
);
wire
->requestFrom
(0x36,
(uint8_t
)2
);
for
(byte i=0; i < 2; i++
) {
readArray[i] = wire
->read
();
}
int _bit_resolution=12;
int _bits_used_msb=11
-7;
float cpr = pow
(2, _bit_resolution
);
int lsb_used = _bit_resolution
- _bits_used_msb;
uint8_t lsb_mask =
(uint8_t
)( (2 << lsb_used
) - 1
);
uint8_t msb_mask =
(uint8_t
)( (2 << _bits_used_msb
) - 1
);
readValue =
( readArray[1] & lsb_mask
);
readValue +=
( ( readArray[0] & msb_mask
) << lsb_used
);
return
(readValue/
(float
)cpr
) * _2PI;
}
//AS5600 相关
//=========角度处理相关=============
Sensor_AS5600::Sensor_AS5600
(int Mot_Num
) {
_Mot_Num=Mot_Num; //使得 Mot_Num 可以统一在该文件调用
}
void Sensor_AS5600::Sensor_init
(TwoWire* _wire
) {
wire=_wire;
wire
->begin
(); //电机Sensor
delay
(500
);
getSensorAngle
();
delayMicroseconds
(1
);
vel_angle_prev = getSensorAngle
();
vel_angle_prev_ts = micros
();
delay
(1
);
getSensorAngle
();
delayMicroseconds
(1
);
angle_prev = getSensorAngle
();
angle_prev_ts = micros
();
}
void Sensor_AS5600::Sensor_update
() {
float val = getSensorAngle
();
angle_prev_ts = micros
();
float d_angle = val
- angle_prev;
// 圈数检测
if
(abs
(d_angle
) >
(0.8f*_2PI
) ) full_rotations +=
( d_angle > 0
) ?
-1 : 1;
angle_prev = val;
}
float Sensor_AS5600::getMechanicalAngle
() {
return angle_prev;
}
float Sensor_AS5600::getAngle
(){
return
(float
)full_rotations * _2PI + angle_prev;
}
float Sensor_AS5600::getVelocity
() {
// 计算采样时间
float Ts =
(angle_prev_ts
- vel_angle_prev_ts
)*1e
-6;
// 快速修复奇怪的情况（微溢出）
if
(Ts <= 0
) Ts = 1e
-3f;
// 速度计算
float vel =
( (float
)(full_rotations
- vel_full_rotations
)*_2PI +
(angle_prev
- vel_angle_prev
) ) / Ts;
// 保存变量以待将来使用
vel_angle_prev = angle_prev;
vel_full_rotations = full_rotations;
vel_angle_prev_ts = angle_prev_ts;
return vel;
}
#include <Arduino.h>
#include "Wire.h"
class Sensor_AS5600
{
public:
Sensor_AS5600
(int Mot_Num
);
void Sensor_init
(TwoWire* _wire = &Wire
);
void Sensor_update
();
float getAngle
();
float getVelocity
();
float getMechanicalAngle
();
double getSensorAngle
();
private:
int _Mot_Num;
//AS5600 变量定义
//int sensor_direction=1; //编码器旋
转方向定义
float angle_prev=0; // 最后一次调用 getSensorAngle
() 的输出结果，用于得到完整的圈数和速度
long angle_prev_ts=0; // 上次调用 getAngle 的时间戳
float vel_angle_prev=0; // 最后一次调用 getVelocity 时的角度
long vel_angle_prev_ts=0; // 最后速度计算时间戳
int32_t full_rotations=0; // 总圈数计数
int32_t vel_full_rotations=0; //用于速度计算的先前完整旋
转圈数
TwoWire* wire;
};
#include <Arduino.h>
#include "AS5600.h"
#include "lowpass_filter.h"
#include "pid.h"
#define _constrain
(amt,low,high
) ((amt
)<
(low
)?
(low
):
((amt
)>
(high
)?
(high
):
(amt
)))
float voltage_power_supply;
float Ualpha,Ubeta=0,Ua=0,Ub=0,Uc=0;
#define _3PI_2 4.71238898038f
float zero_electric_angle=0;
int PP=1,DIR=1;
int
pwmA = 32;
int
pwmB = 33;
int
pwmC = 25;
//低通滤波初始化
LowPassFilter M0_Vel_Flt = LowPassFilter
(0.01
); // Tf = 10ms //M0速度环
//PID
PIDController vel_loop_M0 = PIDController{.P = 2, .I = 0, .D = 0, .ramp = 100000, .limit = voltage_power_supply/2};
PIDController angle_loop_M0 = PIDController{.P = 2, .I = 0, .D = 0, .ramp = 100000, .limit = 100};
//AS5600
Sensor_AS5600 S0=Sensor_AS5600
(0
);
TwoWire S0_I2C = TwoWire
(0
);
//=================PID 设置函数=================
//速度PID
void DFOC_M0_SET_VEL_PID
(float P,float I,float D,float ramp
) //M0角度环PID设置
{
vel_loop_M0.P=P;
vel_loop_M0.I=I;
vel_loop_M0.D=D;
vel_loop_M0.output_ramp=ramp;
}
//角度PID
void DFOC_M0_SET_ANGLE_PID
(float P,float I,float D,float ramp
) //M0角度环PID设置
{
angle_loop_M0.P=P;
angle_loop_M0.I=I;
angle_loop_M0.D=D;
angle_loop_M0.output_ramp=ramp;
}
//M0速度PID接口
float DFOC_M0_VEL_PID
(float error
) //M0速度环
{
return vel_loop_M0
(error
);
}
//M0角度PID接口
float DFOC_M0_ANGLE_PID
(float error
)
{
return angle_loop_M0
(error
);
}
//初始变量及函数定义
#define _constrain
(amt,low,high
) ((amt
)<
(low
)?
(low
):
((amt
)>
(high
)?
(high
):
(amt
)))
//宏定义实现的一个约束函数,用于限制一个值的范围。
//具体来说，该宏定义的名称为 _constrain，接受三个参数 amt、low 和 high，分别表示要限制的值、最小值和最大值。该宏定义的实现使用了三元运算符，根据 amt 是否小于 low 或大于 high，返回其
中的最大或最小值，或者返回原值。
//换句话说，如果 amt 小于 low，则返回 low；如果 amt 大于 high，则返回 high；否则返回 amt。这样，_constrain
(amt, low, high
) 就会将 amt 约束在 [low, high] 的范围内。1
// 归一化角度到 [0,2PI]
float _normalizeAngle
(float angle
){
float a = fmod
(angle, 2*PI
); //取余运算可以用于归一化，列出特殊值例子算便知
return a >= 0 ? a :
(a + 2*PI
);
//三目运算符。格式：condition ? expr1 : expr2
//其中，condition 是要求值的条件表达式，如果条件成立，则返回 expr1 的值，否则返回 expr2 的值。可以将三目运算符视为 if
-else 语句的简化形式。
//fmod 函数的余数的符号与除数相同。因此，当 angle 的值为负数时，余数的符号将与 _2PI 的符号相反。也就是说，如果 angle 的值小于 0 且 _2PI 的值为正数，则 fmod
(angle, _2PI
) 的余数将为负数。
//例如，当 angle 的值为
-PI/2，_2PI 的值为 2PI 时，fmod
(angle, _2PI
) 将返回一个负数。在这种情况下，可以通过将负数的余数加上 _2PI 来将角度归一化到 [0, 2PI] 的范围内，以确保角度的值始终为正数。
}
// 设置
PWM到
控制器输出
void set
Pwm(float Ua, float Ub, float Uc
) {
// 限制上限
Ua = _constrain
(Ua, 0.0f, voltage_power_supply
);
Ub = _constrain
(Ub, 0.0f, voltage_power_supply
);
Uc = _constrain
(Uc, 0.0f, voltage_power_supply
);
// 计算占空比
// 限制占空比从0到1
float dc_a = _constrain
(Ua / voltage_power_supply, 0.0f , 1.0f
);
float dc_b = _constrain
(Ub / voltage_power_supply, 0.0f , 1.0f
);
float dc_c = _constrain
(Uc / voltage_power_supply, 0.0f , 1.0f
);
//写入
PWM到
PWM 0 1 2 通道
ledcWrite
(0, dc_a*255
);
ledcWrite
(1, dc_b*255
);
ledcWrite
(2, dc_c*255
);
}
void setTorque
(float Uq,float angle_el
) {
S0.Sensor_update
(); //更新传感器数值
Uq=_constrain
(Uq,
-(voltage_power_supply
)/2,
(voltage_power_supply
)/2
);
float Ud=0;
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;
set
Pwm(Ua,Ub,Uc
);
}
void DFOC_Vbus
(float power_supply
)
{
voltage_power_supply=power_supply;
pinMode
(pwmA, OUTPUT
);
pinMode
(pwmB, OUTPUT
);
pinMode
(pwmC, OUTPUT
);
ledcAttachPin
(pwmA, 0
);
ledcAttachPin
(pwmB, 1
);
ledcAttachPin
(pwmC, 2
);
ledcSetup
(0, 30000, 8
); //
pwm频道, 频率, 精度
ledcSetup
(1, 30000, 8
); //
pwm频道, 频率, 精度
ledcSetup
(2, 30000, 8
); //
pwm频道, 频率, 精度
Serial.println
("完成
PWM初始化设置"
);
//AS5600
S0_I2C.begin
(19,18, 400000UL
);
S0.Sensor_init
(&S0_I2C
); //初始化编码器0
Serial.println
("编码器加载完毕"
);
//PID 加载
vel_loop_M0 = PIDController{.P = 2, .I = 0, .D = 0, .ramp = 100000, .limit = voltage_power_supply/2};
}
float _electricalAngle
(){
return _normalizeAngle
((float
)(DIR * PP
) * S0.getMechanicalAngle
()-zero_electric_angle
);
}
void DFOC_alignSensor
(int _PP,int _DIR
)
{
PP=_PP;
DIR=_DIR;
setTorque
(3, _3PI_2
); //起劲
delay
(1000
);
S0.Sensor_update
(); //更新角度，方便下面电角度读取
zero_electric_angle=_electricalAngle
();
setTorque
(0, _3PI_2
); //松劲（解除校准）
Serial.print
("0电角度："
);Serial.println
(zero_electric_angle
);
}
float DFOC_M0_Angle
()
{
return DIR*S0.getAngle
();
}
//无滤波
//float DFOC_M0_Velocity
()
//{
// return DIR*S0.getVelocity
();
//}
//有滤波
float DFOC_M0_Velocity
()
{
//获取速度数据并滤波
float vel_M0_ori=S0.getVelocity
();
float vel_M0_flit=M0_Vel_Flt
(DIR*vel_M0_ori
);
return vel_M0_flit; //考虑方向
}
//==============串口接收==============
float motor_target;
int commaPosition;
String serialReceiveUserCommand
() {
// a string to hold incoming data
static String received_chars;
String command = "";
while
(Serial.available
()) {
// get the new byte:
char inChar =
(char
)Serial.read
();
// add it to the string buffer:
received_chars += inChar;
// end of user input
if
(inChar == '\n'
) {
// execute the user command
command = received_chars;
commaPosition = command.indexOf
('\n'
);//检测字符串
中的逗号
if
(commaPosition !=
-1
)//如果有逗号存在就向下执行
{
motor_target = command.substring
(0,commaPosition
).toDouble
(); //电机角度
Serial.println
(motor_target
);
}
// reset the command buffer
received_chars = "";
}
}
return command;
}
float serial_motor_target
()
{
return motor_target;
}
//================简易接口函数================
void DFOC_M0_set_Velocity_Angle
(float Target
)
{
setTorque
(DFOC_M0_VEL_PID
(DFOC_M0_ANGLE_PID
((Target
-DFOC_M0_Angle
())*180/PI
)),_electricalAngle
()); //角度闭环
}
void DFOC_M0_setVelocity
(float Target
)
{
setTorque
(DFOC_M0_VEL_PID
((serial_motor_target
()-DFOC_M0_Velocity
())*180/PI
),_electricalAngle
()); //速度闭环
}
void DFOC_M0_set_Force_Angle
(float Target
) //力位
{
setTorque
(DFOC_M0_ANGLE_PID
((Target
-DFOC_M0_Angle
())*180/PI
),_electricalAngle
());
}
void DFOC_M0_setTorque
(float Target
)
{
setTorque
(Target,_electricalAngle
());
}
void set
Pwm(float Ua, float Ub, float Uc
);
float setTorque
(float Uq,float angle_el
);
float _normalizeAngle
(float angle
);
void DFOC_Vbus
(float power_supply
);
void DFOC_alignSensor
(int _PP,int _DIR
);
float _electricalAngle
();
float serial_motor_target
();
String serialReceiveUserCommand
();
//传感器读取
float DFOC_M0_Velocity
();
float DFOC_M0_Angle
();
//PID
void DFOC_M0_SET_ANGLE_PID
(float P,float I,float D,float ramp
);
void DFOC_M0_SET_VEL_PID
(float P,float I,float D,float ramp
);
float DFOC_M0_VEL_PID
(float error
);
float DFOC_M0_ANGLE_PID
(float error
);
//接口函数
void DFOC_M0_set_Velocity_Angle
(float Target
);
void DFOC_M0_setVelocity
(float Target
);
void DFOC_M0_set_Force_Angle
(float Target
);
void DFOC_M0_setTorque
(float Target
);
#include "lowpass_filter.h"
LowPassFilter::LowPassFilter
(float time_constant
)
: Tf
(time_constant
)
, y_prev
(0.0f
)
{
timestamp_prev = micros
();
}
float LowPassFilter::operator
() (float x
)
{
unsigned long timestamp = micros
();
float dt =
(timestamp
- timestamp_prev
)*1e
-6f;
if
(dt < 0.0f
) dt = 1e
-3f;
else if
(dt > 0.3f
) {
y_prev = x;
timestamp_prev = timestamp;
return x;
}
float alpha = Tf/
(Tf + dt
);
float y = alpha*y_prev +
(1.0f
- alpha
)*x;
y_prev = y;
timestamp_prev = timestamp;
return y;
}
#include <Arduino.h>
#ifndef LOWPASS_FILTER_H
#define LOWPASS_FILTER_H
/**
* 低通滤波器类
*/
class LowPassFilter
{
public:
/**
* @Tf
- 低通滤波时间常数
*/
LowPassFilter
(float Tf
);
~LowPassFilter
() = default;
float operator
() (float x
);
float Tf; //!< 低通滤波时间常数
protected:
unsigned long timestamp_prev; //!< 最后执行时间戳
float y_prev; //!< 上一个循环
中的过滤后的值
};
#endif
#include "pid.h"
#include <Arduino.h>
#define _constrain
(amt,low,high
) ((amt
)<
(low
)?
(low
):
((amt
)>
(high
)?
(high
):
(amt
)))
PIDController::PIDController
(float P, float I, float D, float ramp, float limit
)
: P
(P
)
, I
(I
)
, D
(D
)
, output_ramp
(ramp
) // PID
控制器加速度限幅
, limit
(limit
) // PID
控制器输出限幅
, error_prev
(0.0f
)
, output_prev
(0.0f
)
, integral_prev
(0.0f
)
{
timestamp_prev = micros
();
}
// PID
控制器函数
float PIDController::operator
() (float error
){
// 计算两次循环中间的间隔时间
unsigned long timestamp_now = micros
();
float Ts =
(timestamp_now
- timestamp_prev
) * 1e
-6f;
if
(Ts <= 0 || Ts > 0.5f
) Ts = 1e
-3f;
// P环
float proportional = P * error;
// Tustin 散点积分（I环）
float integral = integral_prev + I*Ts*0.5f*
(error + error_prev
);
integral = _constrain
(integral,
-limit, limit
);
// D环（微分环节）
float derivative = D*
(error
- error_prev
)/Ts;
// 将P,I,D三环的计算值加起来
float output = proportional + integral + derivative;
output = _constrain
(output,
-limit, limit
);
if
(output_ramp > 0
){
// 对PID的变化速率进行限制
float output_rate =
(output
- output_prev
)/Ts;
if
(output_rate > output_ramp
)
output = output_prev + output_ramp*Ts;
else if
(output_rate <
-output_ramp
)
output = output_prev
- output_ramp*Ts;
}
// 保存值（为了下一次循环）
integral_prev = integral;
output_prev = output;
error_prev = error;
timestamp_prev = timestamp_now;
return output;
}
#ifndef PID_H
#define PID_H
class PIDController
{
public:
PIDController
(float P, float I, float D, float ramp, float limit
);
~PIDController
() = default;
float operator
() (float error
);
float P; //!< 比例增益
(P环增益
)
float I; //!< 积分增益（I环增益）
float D; //!< 微分增益（D环增益）
float output_ramp;
float limit;
protected:
float error_prev; //!< 最后的跟踪误差值
float output_prev; //!< 最后一个 pid 输出值
float integral_prev; //!< 最后一个积分分量值
unsigned long timestamp_prev; //!< 上次执行时间戳
};
#endif
void set
Pwm(float Ua, float Ub, float Uc
);
float setTorque
(float Uq,float angle_el
);
float _normalizeAngle
(float angle
);
void DFOC_Vbus
(float power_supply
);
void DFOC_alignSensor
(int _PP,int _DIR
);
float _electricalAngle
();
float serial_motor_target
();
String serialReceiveUserCommand
();
//传感器读取
float DFOC_M0_Velocity
();
float DFOC_M0_Angle
();
//PID
void DFOC_M0_SET_ANGLE_PID
(float P,float I,float D,float ramp
);
void DFOC_M0_SET_VEL_PID
(float P,float I,float D,float ramp
);
float DFOC_M0_VEL_PID
(float error
);
float DFOC_M0_ANGLE_PID
(float error
);
//接口函数
void DFOC_M0_set_Velocity_Angle
(float Target
);
void DFOC_M0_setVelocity
(float Target
);
void DFOC_M0_set_Force_Angle
(float Target
);
void DFOC_M0_setTorque
(float Target
);
重写下面的代码使它运行后的效果与上方代码一致
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file : main.c
* @brief : Main program body
******************************************************************************
* @attention
*
* Copyright
(c
) 2025 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS
-IS.
*
******************************************************************************
*/
/* USER CODE END Header */
/* Includes
------------------------------------------------------------------*/
#include "main.h"
#include "i2c.h"
#include "tim.h"
#include "usart.h"
#include "wwdg.h"
#include "
gpio.h"
/* Private includes
----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "BLDC.h"
#include "string.h"
#include "AS5600.h"
#include "stdio.h"
#include "stdlib.h"
//#include "stdio.h"
//#include "string.h"
//#include "stdlib.h"
//#include "math.h"
/* USER CODE END Includes */
/* Private typedef
-----------------------------------------------------------*/
/* USER CODE BEGIN PTD */
//char msg [100];
/* USER CODE END PTD */
/* Private define
------------------------------------------------------------*/
/* USER CODE BEGIN PD */
/* USER CODE END PD */
/* Private macro
-------------------------------------------------------------*/
/* USER CODE BEGIN PM */
/* USER CODE END PM */
/* Private variables
---------------------------------------------------------*/
/* USER CODE BEGIN PV */
// 速度
控制相关变量
float target_speed = 0.0f; // 目标速度
(rad/s
)
float current_speed = 0.0f; // 实际速度
(rad/s
)
uint8_t uart_rx_buf[32]; // 串口接收缓冲区
uint8_t uart_rx_len = 0; // 接收数据长度
/* USER CODE END PV */
/* USER CODE BEGIN PFP */
void SpeedControl_Update
(void
);
void HAL_UART_RxCpltCallback
(UART_HandleTypeDef *huart
);
/* USER CODE END PV */
/* Private function prototypes
-----------------------------------------------*/
void
SystemClock_Config
(void
);
/* USER CODE BEGIN PFP */
/* USER CODE END PFP */
/* Private user code
---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
/* USER CODE END 0 */
/**
* @brief The application entry point.
* @retval int
*/
int main
(void
)
{
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */
/* MCU Configuration
--------------------------------------------------------*/
/* Reset of all peripherals, Initializes the Flash interface and the
Systick. */
HAL_Init
();
/* USER CODE BEGIN Init */
/* USER CODE END Init */
/* Configure the
system clock */
SystemClock_Config
();
/* USER CODE BEGIN
SysInit */
/* USER CODE END
SysInit */
/* Initialize all configured peripherals */
MX_
GPIO_Init
();
MX_TIM3_Init
();
MX_TIM17_Init
();
//MX_WWDG_Init
();
MX_I2C1_Init
();
MX_USART1_UART_Init
();
/* USER CODE BEGIN 2 */
// 启动串口接收中断
HAL_UART_Receive_IT
(&huart1, &uart_rx_buf[uart_rx_len], 1
);
Init
();
/* USER CODE END 2 */
/* Infinite loop */
/* USER CODE BEGIN WHILE */
while
(1
)
{
/* USER CODE END WHILE */
Cycle
();
SpeedControl_Update
(); // 添加速度
控制
HAL_Delay
(1
); // 1ms
控制周期
// int len = sprintf
(msg, "Uq=%.2fV, Angle=%.1f°, Speed=%.1frad/s\r\n",
// uq_div,
// has5600.angle,
// has5600.speed
);
// HAL_UART_Transmit
(&huart1,
(uint8_t*
)tx_buffer, len, HAL_MAX_DELAY
);
/* USER CODE BEGIN 3 */
}
/* USER CODE END 3 */
}
/**
* @brief
System Clock Configuration
* @retval None
*/
void
SystemClock_Config
(void
)
{
RCC_OscInitTypeDef RCC_OscInitStruct = {0};
RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
/** Configure the main internal regulator output voltage
*/
HAL_PWREx_ControlVoltageScaling
(PWR_REGULATOR_VOLTAGE_SCALE1
);
/** Initializes the RCC Oscillators according to the specified parameters
* in the RCC_OscInitTypeDef structure.
*/
RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI;
RCC_OscInitStruct.HSIState = RCC_HSI_ON;
RCC_OscInitStruct.HSIDiv = RCC_HSI_DIV1;
RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI;
RCC_OscInitStruct.PLL.PLLM = RCC_PLLM_DIV1;
RCC_OscInitStruct.PLL.PLLN = 8;
RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
RCC_OscInitStruct.PLL.PLLR = RCC_PLLR_DIV2;
if
(HAL_RCC_OscConfig
(&RCC_OscInitStruct
) != HAL_OK
)
{
Error_Handler
();
}
/** Initializes the CPU, AHB and APB buses clocks
*/
RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_
SYSCLK
|RCC_CLOCKTYPE_PCLK1;
RCC_ClkInitStruct.
SYSCLKSource = RCC_
SYSCLKSOURCE_PLLCLK;
RCC_ClkInitStruct.AHBCLKDivider = RCC_
SYSCLK_DIV1;
RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV1;
if
(HAL_RCC_ClockConfig
(&RCC_ClkInitStruct, FLASH_LATENCY_2
) != HAL_OK
)
{
Error_Handler
();
}
}
/* USER CODE BEGIN 4 */
/* USER CODE END 4 */
/**
* @brief This function is executed in case of error occurrence.
* @retval None
*/
void Error_Handler
(void
)
{
/* USER CODE BEGIN Error_Handler_Debug */
/* User can add his own implementation to report the HAL error return state */
__disable_irq
();
while
(1
)
{
}
/* USER CODE END Error_Handler_Debug */
}
#ifdef USE_FULL_ASSERT
/**
* @brief Reports the name of the source file and the source line number
* where the assert_param error has occurred.
* @param file: pointer to the source file name
* @param line: assert_param error line source number
* @retval None
*/
void assert_failed
(uint8_t *file, uint32_t line
)
{
/* USER CODE BEGIN 6 */
/* User can add his own implementation to report the file name and line number,
ex: printf
("Wrong parameters value: file %s on line %d\r\n", file, line
) */
/* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file : main.h
* @brief : Header for main.c file.
* This file contains the common defines of the application.
******************************************************************************
* @attention
*
* Copyright
(c
) 2025 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS
-IS.
*
******************************************************************************
*/
/* USER CODE END Header */
/* Define to prevent recursive inclusion
-------------------------------------*/
#ifndef __MAIN_H
#define __MAIN_H
#ifdef __cplusplus
extern "C" {
#endif
/* Includes
------------------------------------------------------------------*/
#include "stm32g0xx_hal.h"
/* Private includes
----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
/* USER CODE END Includes */
/* Exported types
------------------------------------------------------------*/
/* USER CODE BEGIN ET */
/* USER CODE END ET */
/* Exported constants
--------------------------------------------------------*/
/* USER CODE BEGIN EC */
/* USER CODE END EC */
/* Exported macro
------------------------------------------------------------*/
/* USER CODE BEGIN EM */
/* USER CODE END EM */
/* Exported functions prototypes
---------------------------------------------*/
void Error_Handler
(void
);
/* USER CODE BEGIN EFP */
/* USER CODE END EFP */
/* Private defines
-----------------------------------------------------------*/
#define LED_RUN_Pin
GPIO_PIN_14
#define LED_RUN_
GPIO_Port
GPIOC
#define KEY_Pin
GPIO_PIN_15
#define KEY_
GPIO_Port
GPIOC
#define KEY_EXTI_IRQn EXTI4_15_IRQn
#define SLEEP_Pin
GPIO_PIN_0
#define SLEEP_
GPIO_Port
GPIOA
#define RESET_Pin
GPIO_PIN_1
#define RESET_
GPIO_Port
GPIOA
#define ERROR_Pin
GPIO_PIN_2
#define ERROR_
GPIO_Port
GPIOA
#define ERROR_EXTI_IRQn EXTI2_3_IRQn
#define EN1_Pin
GPIO_PIN_3
#define EN1_
GPIO_Port
GPIOA
#define EN2_Pin
GPIO_PIN_4
#define EN2_
GPIO_Port
GPIOA
#define EN3_Pin
GPIO_PIN_5
#define EN3_
GPIO_Port
GPIOA
/* USER CODE BEGIN Private defines */
/* USER CODE END Private defines */
#ifdef __cplusplus
}
#endif
#endif /* __MAIN_H */
#include "i2c.h"
#include "math.h"
#include "AS5600.h"
#include "stdio.h"
#include "string.h"
#include "usart.h"
#include "LPF.h"
#define AS5600_PORT &hi2c1
#define AS5600_I2C_TIMEOUT 5
#define AS5600_ADDRESS 0x36 // 7
-bit address
#define AS5600_RAW_ANGLE_REG 0x0C // Raw angle register
(12
-bit
)
#define _2PI 6.28318530718f
AS5600_HandleTypeDef has5600;
LPF_1Order speed_filter; // 速度低通滤波器
// 初始化AS5600传感器
void AS5600_Init
(void
) {
// 初始化滤波器（截止频率10Hz，假设采样周期1ms）
LPF_1Order_Init
(&speed_filter, 10.0f, 0.001f
);
// 读取初始角度（丢弃第一次可能不稳定的读数）
AS5600_Read_RawAngle
(&has5600
);
has5600.last_raw_angle = has5600.raw_angle;
has5600.full_rotations = 0;
}
// 读取原始角度（0
-4095）
void AS5600_Read_RawAngle
(AS5600_HandleTypeDef *h
) {
uint8_t data[2];
HAL_I2C_Mem_Read
(AS5600_PORT, AS5600_ADDRESS,
AS5600_RAW_ANGLE_REG, I2C_MEMADD_SIZE_8BIT,
data, 2, AS5600_I2C_TIMEOUT
);
h
->raw_angle =
(data[0] << 8
) | data[1]; // 组合高低字节
}
// 更新角度和速度计算
void AS5600_Update
(AS5600_HandleTypeDef *h
) {
static uint32_t last_time = 0;
uint32_t current_time = HAL_GetTick
();
float dt =
(current_time
- last_time
) * 0.001f; //
转换为秒
last_time = current_time;
// 保存上一次角度
float prev_angle = h
->angle_rad;
// 读取新角度
AS5600_Read_RawAngle
(h
);
// 处理角度环绕（4095→0跳变）
int16_t delta = h
->raw_angle
- h
->last_raw_angle;
if
(delta <
-2048
) delta += 4096;
else if
(delta > 2048
) delta
-= 4096;
//
转换为弧度（0
-2π）
h
->angle_rad =
(float
)h
->raw_angle / 4096.0f * _2PI;
h
->angle_deg = h
->angle_rad * 180.0f / 3.14159265f;
// 多圈跟踪（角度突变超过0.8圈时调整圈数）
float angle_diff = h
->angle_rad
- prev_angle;
if
(fabsf
(angle_diff
) > 0.8f * _2PI
) {
h
->full_rotations +=
(angle_diff > 0
) ?
-1 : 1;
}
// 计算角速度（rad/s）
if
(dt > 0
) {
float raw_speed = angle_diff / dt;
h
->speed_radps = LPF_1Order_Update
(&speed_filter, raw_speed
);
}
h
->last_raw_angle = h
->raw_angle;
}
// 获取累计多圈角度（弧度）
float AS5600_GetFullAngle
(AS5600_HandleTypeDef *h
) {
return h
->full_rotations * _2PI + h
->angle_rad;
}
// 通过串口输出数据
void AS5600_Send_Data
(AS5600_HandleTypeDef *h
) {
char buffer[64];
snprintf
(buffer, sizeof
(buffer
),
"Angle: %.2f deg, Speed: %.2f rad/s, Turns: %ld\r\n",
h
->angle_deg, h
->speed_radps, h
->full_rotations
);
HAL_UART_Transmit
(&huart1,
(uint8_t*
)buffer, strlen
(buffer
), HAL_MAX_DELAY
);
}
// 主循环调用示例
void AS5600_Cycle
(void
) {
static uint32_t last_update = 0;
if
(HAL_GetTick
() - last_update >= 10
) { // 每10ms更新一次
AS5600_Update
(&has5600
);
AS5600_Send_Data
(&has5600
);
last_update = HAL_GetTick
();
}
}
#include "main.h"
#define AS5600_D_CW 1
#define AS5600_D_CCW 0
#define AS5600_D_STOP 2
// 传感器数据结构体
typedef struct {
uint16_t raw_angle; // 原始角度值
(0
-4095
)
uint16_t last_raw_angle; // 上一次原始角度值
float angle_rad; // 当前角度（弧度）
float angle_deg; // 当前角度（度）
float speed_radps; // 角速度（rad/s）
int32_t full_rotations; // 完整旋
转圈数
} AS5600_HandleTypeDef;
extern uint16_t as5600_read_cnt;
extern AS5600_HandleTypeDef has5600;
void AS5600_Init
(void
);
void AS5600_Cycle
(void
);
uint16_t AS5600_Read_uint16
(uint8_t reg_addr
);
void AS5600_Refresh_Angle
(AS5600_HandleTypeDef *as5600_handle
);
void AS5600_Send_Data
(AS5600_HandleTypeDef *as5600_handle
);
void AS5600_Read_RawAngle
(AS5600_HandleTypeDef *h
);
#endif /* __A_AS5600_H */
#ifndef __BLDC_H
#define __BLDC_H
#include "main.h"
#define Constrain
(amt,low,high
) ((amt
)<
(low
)?
(low
):
((amt
)>
(high
)?
(high
):
(amt
)))
#define POLE_PAIRS 7
extern uint16_t debug_cnt;
extern uint16_t oled_cnt;
void Init
(void
);
void Cycle
(void
);
void My_BLDC_Init
(void
);
void Set_
PWM(float Ua, float Ub, float Uc
);
float Normalize_Angle
(float angle
);
float Elect_Angle
(float shaft_angle, int pole_pairs
);
void Set_Phase_Voltage
(float Uq,float Ud, float angle_el
);
#endif /* __A_BLDC_MAIN_H */
//=====
Sys =====
#include "stdio.h"
#include "string.h"
#include "stdlib.h"
#include "math.h"
//===== ST =====
#include "tim.h"
#include "usart.h"
#include "wwdg.h"
//===== Self =====
#include "BLDC.h"
#include "BLDC_tim.h"
#include "DRV8313.h"
#include "AS5600.h"
//===== Ext Lib File =====
#include "MyProject.h"
//===== IO Config =====
#define RUN_LED_ON
() HAL_
GPIO_WritePin
(LED_RUN_
GPIO_Port, LED_RUN_Pin,
GPIO_PIN_SET
)
#define RUN_LED_OFF
() HAL_
GPIO_WritePin
(LED_RUN_
GPIO_Port, LED_RUN_Pin,
GPIO_PIN_RESET
)
#define RUN_LED_TOGGLE
() HAL_
GPIO_TogglePin
(LED_RUN_
GPIO_Port, LED_RUN_Pin
);
//===== BLDC Config =====
uint8_t mode = 0;
float roll_speed = 6.28;
float target;
//char tx_buffer [100];
void Init
(void
){
HAL_Delay
(5
);
DRV8313_Init
();
AS5600_Init
();
HAL_Delay
(50
);
My_BLDC_Init
();
}
void Cycle
(void
){
AS5600_Cycle
();
}
uint16_t bldc_cnt = 50;
float
pwm_duty = 0.1f;
float shaft_Angle=0,
zero_elect_angle=0,
Ualpha,Ubeta=0,
Ua=0,Ub=0,Uc=0,
dc_a=0,dc_b=0,dc_c=0;
float target_velocity = 10, vel_lim = 80;
float vin = 12.6;
float uq_div = 5; //Uq = vin/uq_div, Uq must small than vin/3.
unsigned long timeStamp;
void My_BLDC_Init
(void
){
mode=1;
RUN_LED_ON
();
DRV8313_ENx_On
();
HAL_TIM_
PWM_Start
(&htim3, TIM_CHANNEL_1
);
HAL_TIM_
PWM_Start
(&htim3, TIM_CHANNEL_2
);
HAL_TIM_
PWM_Start
(&htim3, TIM_CHANNEL_3
);
// mode=0;
// RUN_LED_OFF
();
// DRV8313_ENx_Off
();
// HAL_TIM_
PWM_Stop
(&htim3, TIM_CHANNEL_1
);
// HAL_TIM_
PWM_Stop
(&htim3, TIM_CHANNEL_2
);
// HAL_TIM_
PWM_Stop
(&htim3, TIM_CHANNEL_3
);
}
float Elect_Angle
(float shaft_Angle, int pole_pairs
){
return
(shaft_Angle*pole_pairs
);
}
float Normalize_Angle
(float angle
){
float ret = fmod
(angle, _2PI
);
if
(ret >= 0
){
return ret;
}else{
return
(ret + _2PI
);
}
}
void Set_Phase_Voltage
(float Uq,float Ud, float angle_el
) {
angle_el = Normalize_Angle
(angle_el + zero_elect_angle
);
// 帕克逆变换
Ualpha =
-Uq*sin
(angle_el
);
Ubeta = Uq*cos
(angle_el
);
// 克拉克逆变换
Ua = Ualpha + vin/2;
Ub =
(sqrt
(3
)*Ubeta
-Ualpha
)/2 + vin/2;
Uc =
(-Ualpha
-sqrt
(3
)*Ubeta
)/2 + vin/2;
Set_
PWM(Ua,Ub,Uc
);
}
void Set_
PWM(float Ua, float Ub, float Uc
){
// 计算占空比
// 限制占空比从0到1
dc_a = Constrain
(Ua / vin, 0.0f , 1.0f
);
dc_b = Constrain
(Ub / vin, 0.0f , 1.0f
);
dc_c = Constrain
(Uc / vin, 0.0f , 1.0f
);
Set
PWMDutyPersent
(&htim3, TIM_CHANNEL_1, dc_a*100
);
Set
PWMDutyPersent
(&htim3, TIM_CHANNEL_2, dc_b*100
);
Set
PWMDutyPersent
(&htim3, TIM_CHANNEL_3, dc_c*100
);
}
void HAL_
GPIO_EXTI_Rising_Callback
(uint16_t
GPIO_Pin
) {
if
(GPIO_Pin == KEY_Pin
) {
if
(HAL_
GPIO_ReadPin
(KEY_
GPIO_Port, KEY_Pin
) ==
GPIO_PIN_SET
) {
if
(mode == 0
){
mode = 1;
}else{
mode = 0;
}
}
}
}
void HAL_
GPIO_EXTI_Falling_Callback
(uint16_t
GPIO_Pin
) {
if
(GPIO_Pin == ERROR_Pin
){ //BLDC_ERR_Protect
DRV8313_ENx_Off
();
HAL_TIM_
PWM_Stop
(&htim3, TIM_CHANNEL_1
);
HAL_TIM_
PWM_Stop
(&htim3, TIM_CHANNEL_2
);
HAL_TIM_
PWM_Stop
(&htim3, TIM_CHANNEL_3
);
mode = 0;
RUN_LED_OFF
();
}
}
#include "LPF.h"
#include "MyProject.h"
void LPF_1Order_Init
(LPF_1Order *filter, float cutoff_freq, float sample_time
) {
// 计算滤波系数 α = ΔT /
(1/2πfc + ΔT
)
float rc = 1.0f /
(2.0f * _PI * cutoff_freq
);
filter
->alpha = sample_time /
(rc + sample_time
);
filter
->prev_output = 0.0f;
}
float LPF_1Order_Update
(LPF_1Order *filter, float input
) {
filter
->prev_output = filter
->alpha * input +
(1.0f
- filter
->alpha
) * filter
->prev_output;
return filter
->prev_output;
}
#ifndef __LPF_H
#define __LPF_H
typedef struct {
float alpha; // 滤波系数
(α = ΔT /
(RC + ΔT
))
float prev_output; // 上一次的输出值
} LPF_1Order;
void LPF_1Order_Init
(LPF_1Order *filter, float cutoff_freq, float sample_time
);
float LPF_1Order_Update
(LPF_1Order *filter, float input
);
#endif
#include "PID.h"
#ifndef __PID_H
#define __PID_H
#endif
#ifndef __A_DRV8313_H
#define __A_DRV8313_H
#include "main.h"
#define DRV8313_RESET_HIGH
() HAL_
GPIO_WritePin
(RESET_
GPIO_Port, RESET_Pin,
GPIO_PIN_SET
)
#define DRV8313_RESET_LOW
() HAL_
GPIO_WritePin
(RESET_
GPIO_Port, RESET_Pin,
GPIO_PIN_RESET
)
#define DRV8313_SLEEP_HIGH
() HAL_
GPIO_WritePin
(SLEEP_
GPIO_Port, SLEEP_Pin,
GPIO_PIN_SET
)
#define DRV8313_SLEEP_LOW
() HAL_
GPIO_WritePin
(SLEEP_
GPIO_Port, SLEEP_Pin,
GPIO_PIN_RESET
)
#define DRV8313_EN1_HIGH
() HAL_
GPIO_WritePin
(EN1_
GPIO_Port, EN1_Pin,
GPIO_PIN_SET
)
#define DRV8313_EN1_LOW
() HAL_
GPIO_WritePin
(EN1_
GPIO_Port, EN1_Pin,
GPIO_PIN_RESET
)
#define DRV8313_EN2_HIGH
() HAL_
GPIO_WritePin
(EN2_
GPIO_Port, EN2_Pin,
GPIO_PIN_SET
)
#define DRV8313_EN2_LOW
() HAL_
GPIO_WritePin
(EN2_
GPIO_Port, EN2_Pin,
GPIO_PIN_RESET
)
#define DRV8313_EN3_HIGH
() HAL_
GPIO_WritePin
(EN3_
GPIO_Port, EN3_Pin,
GPIO_PIN_SET
)
#define DRV8313_EN3_LOW
() HAL_
GPIO_WritePin
(EN3_
GPIO_Port, EN3_Pin,
GPIO_PIN_RESET
)
void DRV8313_Init
(void
);
void DRV8313_ENx_On
(void
);
void DRV8313_ENx_Off
(void
);
#endif /* __A_DRV8313_H */
#include "DRV8313.h"
void DRV8313_Init
(void
){
DRV8313_RESET_LOW
();
HAL_Delay
(1
);
DRV8313_RESET_HIGH
();
HAL_Delay
(1
);
DRV8313_SLEEP_HIGH
();
DRV8313_ENx_Off
();
}
void DRV8313_ENx_On
(void
){
DRV8313_EN1_HIGH
();
DRV8313_EN2_HIGH
();
DRV8313_EN3_HIGH
();
}
void DRV8313_ENx_Off
(void
){
DRV8313_EN1_LOW
();
DRV8313_EN2_LOW
();
DRV8313_EN3_LOW
();
}
//===== STM32 HAL Includes =====
#include "tim.h"
//===== Self Includes =====
#include "BLDC_tim.h"
#include "BLDC.h"
#include "AS5600.h"
//#include "a_lsm6ds3tr.h"
/**
*@brief 微秒阻塞延迟。
*@param delay
- 延迟微秒数。
*@retval 无。
*/
void HAL_Delay_us
(uint32_t delay
){
HAL_
SYSTICK_Config
(64
); //改变
System tick，使HAL_Delay变成微秒延迟。64MHz for us delay
HAL_Delay
(delay
);
HAL_
SYSTICK_Config
(64000
); //改变
System tick，使HAL_Delay变成毫秒延迟。64MHz for ms delay
}
/**
*@brief 设定
PWM的正占空比。
*@param htim
- 定时器句柄；channel
- 定时器通道；dutyPersent
- PWM正占空比0~100。
*@retval 无。
*/
void Set
PWMDutyPersent
(TIM_HandleTypeDef *htim, uint32_t channel, float dutyPersent
){
uint32_t compare, arr;
arr = __HAL_TIM_GetAutoreload
(htim
); //读当前定时器的自动重装载AutoReload
(ARR
)值
compare =
(dutyPersent/100
) *
(arr+1
); //根据当前定时器的ARR值，计算当前定时器的比较compare值
__HAL_TIM_SetCompare
(htim, channel, compare
); //设定比较compare值
//htim
->Instance
->CCR1 = compare;
}
/**
*@brief 获取
PWM的正占空比。
*@param htim
- 定时器句柄；channel
- 定时器通道。
*@retval
PWM正占空比0~100。
*/
float Get
PWMDutyPersent
(TIM_HandleTypeDef *htim, uint32_t channel
){
float dutyPersent;
uint32_t compare, arr;
arr = __HAL_TIM_GetAutoreload
(htim
); //读当前定时器的自动重装载AutoReload
(ARR
)值
compare = __HAL_TIM_GetCompare
(htim, channel
); //读当前定时器的比较compare值
dutyPersent = 100*
((float
)compare /
(arr+1
)); //计算占空比
return dutyPersent;
}
//internal, only for m_tim.c
uint32_t count_100us = 0;
uint16_t count_ms = 0;
uint8_t count_s = 0;
uint8_t count_min = 0;
uint32_t count_h = 0;
//external, for main.c and other
uint32_t count_100us_out = 0;
uint32_t count_ms_out = 0;
uint32_t count_s_out = 0;
uint32_t count_min_out = 0;
void Timer_SetCount
(void
){
if
(count_100us%10 == 0
){
count_ms++;
count_ms_out++;
if
(count_ms >= 1000
){
count_s++;
count_s_out++;
count_ms = 0;
}
if
(count_s >= 60
){
count_min++;
count_min_out++;
count_s=0;
}
if
(count_min >= 60
){
count_h++;
count_min=0;
}
//external start
if
(count_ms_out >= 0xfffffff0
){
count_ms_out = 0;
}
if
(count_s_out >= 0xfffffff0
){
count_s_out = 0;
}
if
(count_min_out >= 0xfffffff0
){
count_min_out = 0;
}
as5600_read_cnt++;
if
(as5600_read_cnt>60000
){
as5600_read_cnt=0;
}
}
}
//will be called by stm32xnxx_it.c
void TIM17_IR
(void
){
count_100us++;
count_100us_out++;
if
(count_100us >= 0xfffffff0
){
count_100us = 0;
}
if
(count_100us_out >= 0xfffffff0
){
count_100us_out = 0;
}
Timer_SetCount
();
}
uint32_t Timer_GetCount_100us
(void
){
return count_100us_out;
}
void Timer_ResetCount_100us
(void
){
count_100us_out = 0;
}
uint32_t Timer_GetCount_ms
(void
){
return count_ms_out;
}
void Timer_ResetCount_ms
(void
){
count_ms_out = 0;
}
uint32_t Timer_GetCount_s
(void
){
return count_s_out;
}
void Timer_ResetCount_s
(void
){
count_s_out = 0;
}
#ifndef __BLDC_TIM_H
#define __BLDC_TIM_H
#include "main.h"
void TIM17_IR
(void
);
void HAL_Delay_us
(uint32_t delay
);
void Set
PWMDutyPersent
(TIM_HandleTypeDef *htim, uint32_t channel, float dutyPersent
);
float Get
PWMDutyPersent
(TIM_HandleTypeDef *htim, uint32_t channel
);
typedef struct
{
uint32_t h;
uint8_t min;
uint8_t s;
uint16_t ms;
}Time_HandleTypeDef;
extern uint32_t count_100us_out;
extern uint32_t count_ms_out;
extern uint32_t count_s_out;
extern uint32_t count_min_out;
void GetRunTime
(Time_HandleTypeDef *ret_time
);
uint32_t Timer_GetCount_100us
(void
);
void Timer_ResetCount_100us
(void
);
uint32_t Timer_GetCount_ms
(void
);
void Timer_ResetCount_ms
(void
);
uint32_t Timer_GetCount_s
(void
);
void Timer_ResetCount_s
(void
);
#endif /* __A_BLDC_TIM_H */
本文详细介绍在Linux环境下如何通过/sys/class/gpio接口控制GPIO，包括GPIO的配置、方向设置、输出电平控制及关闭流程。并提供了C语言实现的LED闪烁和按键输入读取的示例代码。
扫一扫
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
