HAL库配置STM32F1系列定时器驱动步进电机(四)(梯形加减速)
前言
经过之前的一些学习我们已经成功地让电机成功地转了起来,但是在实际应用中这样的电机是很难满足工业上的一些需求的,因为电机在启动和停止时都很难在一瞬间达到目标速度,我们可以从波形图的角度来看,如果我们让电机从0启动然后马上到目标速度再从速度马上停止到0,这样显然是不现实的,会产生丢步的情况,因此接下来要介绍一种电机控制算法:梯形加减速(资料来自正点原子)
梯形加减速原理
通过前面的学习,我们知道了当定时器处于输出比较模式下时,决定脉冲频率的参数是比较值ccr(以下统称Cn),因此控制速度首先我们要能够实时改变比较值,具体来说有三个:
(1)加速阶段时需要多少脉冲数?使用 n1 进行表示
(2)减速阶段要步进多少脉冲数?使用 n2 进行表示
(3)下一个变化的脉冲的周期是多少?使用 T 进行表示
下面我们就开始求解
ccr的求解
首先我们要明确一些已知量:电机步距角α,加速度accel、角速度speed、转动圈数angle*2*Pi、减速度decel,定时器的计数频率Tt(时钟频率/分频系数),不难得出我们一次变化的脉冲时间δt = Cn*Tt,由
θ*t = angle*2*Pi,angle*2*Pi = 0.5*accel*t^2,θ = n*α;
通过这些我们可以推导出:
一个脉冲时间为T𝑛 = √ 2𝛼*n *accel
我们可以将Tn看作一个时刻
那么δt = Cn*Tt = Tn – Tn-1 = √ 2𝛼*n /accel – √ 2𝛼*(n-1) /accel
当n = 0时,C0 = T𝑡 *√ 2*𝑎 / accel
进一步化简得
Cn = C0*((n+1)^0.5 – n^0.5)
但这样的公式还是不太优雅,其计算量也比较大,对于低性能单片机来说可能不太够,例如我的103ZET6就不是很能承受得了,因此我们需要做一些线性化操作来简化公式,利用麦克劳林公式化简后的结果如下
这样Cn的问题就被我们处理好了
但为了再进一步减小计算量以及提高计算的精度,我们还需要做一些浮点数消除操作以及余项相加,这在后面的代码部分会详细看到
中断服务
在求出了Cn以后我们需要设置比较值,这个时候就要在中断中实现,那么我们应该什么时候知道要加速减速还是匀速呢?这个时候就需要通过计算到什么阶段要执行什么模式,对此我们要声明一个计步器变量来判断
但我们又得分成两种情况,一种是加速-匀速-减速,还有一种是加速-减速,前者代表完整地经过了一个理想的运动流程,后者则是因为还没到匀速阶段就需要开始减速,判断什么时候加速减速匀速就需要计算对应的步数节点
加速-减速
这种情况下我们可以得知加速末速度等于减速段初速度,这样可以得到公式
再通过上面n与t的公式我们可以推导出
但这还求不了总步数,我们还需要加上已知的step这一条件
这样就可以求出对应解
加速-匀速-减速
在这种情况下我们可以得知加速步长与加速到匀速状态的步长是相等的,而加速到匀速状态的步长节点的计算步骤如下
我们首先得知运动的距离即角度公式为2*n*α*accel = max_speed^2,剩余其他计算如下
代码部分
看了那么多原理也不一定能一次性消化的完,我们现在结合代码分析一下~
工程搭建
其实和之前比较输出的配置没什么变化,唯一要注意的就是这里的分频系数要设定合理,否则脉冲发送频率过快就会导致电机丢步甚至堵转,为此我还调试了好半天,这里推荐还是找一个有细分功能的驱动器,不用像我一样用L298N,这样还需要自己通过软件设置细分,有些冗余(其实是懒),因此我是直接不考虑细分,即电机转一圈所需脉冲为200
代码部分
对已知参数定义
/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
#define L298NA1(state) HAL_GPIO_WritePin(GPIOF,GPIO_PIN_7,state) //定义信号线对应到电机的每个节拍号
#define L298NA2(state) HAL_GPIO_WritePin(GPIOF,GPIO_PIN_8,state)
#define L298NB1(state) HAL_GPIO_WritePin(GPIOF,GPIO_PIN_9,state)
#define L298NB2(state) HAL_GPIO_WritePin(GPIOF,GPIO_PIN_10,state)
#define SPR 200 //旋转一圈要200个脉冲 等于细分数*步距角*一圈对应的圈数
#define PAI 3.1415926 //圆周率
#define T1_FREQ 1000000 //定时器计数频率
#define ALPHA ((float)(2*PAI/SPR)) //一个脉冲对应的角度
#define A_T_x10 ((float)(10*ALPHA*T1_FREQ)) //乘上10是为了除去浮点数干扰
#define T1_FREQ_148 ((float)((T1_FREQ*0.69)/10)) // 0.69为误差修正值
#define A_SQ ((float)(2*100000*ALPHA)) //和前面同理是为了减少浮点数误差
#define A_x200 ((float)(200*ALPHA)) /* 2*10*10*a/10 */
typedef struct
{
__IO uint8_t run_state; //电机旋转状态
__IO uint8_t dir; //电机旋转方向
__IO int32_t step_ccr; //下个脉冲周期(δt)的比较值
__IO uint32_t decel_start; //开始减速位置
__IO int32_t decel_val; //减速阶段速度
__IO int32_t min_ccr; //速度最快比较值最小时的值
__IO int32_t accel_count; //加减速阶段计数值
} speedRampData;
enum STA
{
STOP = 0, //电机状态
ACCEL, //加速阶段
DECEL, //减速阶段
RUN //匀速阶段
};
enum DIR
{
CCW = 0, //逆时针
CW //顺时针
};
/* USER CODE END PD */
声明变量
/* USER CODE BEGIN PV */
uint8_t Key,dir,En=0;
uint16_t ccr;
speedRampData srd = {STOP,CW,0,0,0,0};
__IO int32_t g_step_position = 0; //记录电机当前位置
__IO uint8_t g_motion_sta = 0; //记录电机运动状态
__IO uint32_t g_add_pulse_count = 0; //记录脉冲个数
__IO uint32_t g_set_speed = 1000; //运行速度
__IO uint32_t g_step_accel = 25; //加速度
__IO uint32_t g_step_decel = 20; //减速度
__IO uint16_t g_step_angle = 50; //设置的圈数
/* USER CODE END PV */
电机单步运行代码
void Pulse(unsigned char nInputData ,unsigned char nDirection) //电机走一个步距角(四个节拍)
{
uint8_t i;
for(i=4;i>1;i--) {
if (nDirection == 1) {
switch (nInputData) {
case 1: {
L298NA1(0);
L298NA2(1);
L298NB1(0);
L298NB2(1);
}
break;
case 2: {
L298NA1(0);
L298NA2(1);
L298NB1(1);
L298NB2(0);
}
break;
case 3: {
L298NA1(1);
L298NA2(0);
L298NB1(1);
L298NB2(0);
}
break;
case 4: {
L298NA1(1);
L298NA2(0);
L298NB1(0);
L298NB2(1);
}
break;
}
}
else if (nDirection == 0) {
switch (nInputData) {
case 1: {
L298NA1(1);
L298NA2(0);
L298NB1(0);
L298NB2(1);
}
break;
case 2: {
L298NA1(1);
L298NA2(0);
L298NB1(1);
L298NB2(0);
}
break;
case 3: {
L298NA1(0);
L298NA2(1);
L298NB1(1);
L298NB2(0);
}
break;
case 4: {
L298NA1(0);
L298NA2(1);
L298NB1(0);
L298NB2(1);
}
break;
}
}
}
}
电机转动一个步距角
void Motor_Start(unsigned char En, char nDirection) //PWM/OC模式,调用前要先打开PWM/OC通道
{
if(En == 0)
return;
uint8_t j;
for(j=0;j<=4;j++)
{
Pulse(j,nDirection);
HAL_Delay(1);
}
}
梯形加减速计算
void Stepper_Create_param(int32_t step,uint32_t accel, uint32_t decel, uint32_t speed)
{
En = 1;
__IO uint16_t tim_count; //达到最大速度时的步数
__IO uint32_t max_lim; //当加速度最大时的最小比较值
__IO uint32_t accel_lim; //加速时的比较值
if(step<0)
{
dir = CCW;
step = -step; //反转方向
}
else
{
dir = CW;
}
if(step == 1) //如果步数为1
{
srd.accel_count = -1; //只移动一步
srd.run_state = DECEL; //减速状态
srd.step_ccr = 1000; //默认速度
}
else if(step!=0)
{
srd.min_ccr = (int32_t)(A_T_x10/speed); //匀速运动时的ccr值,即min_ccr
srd.step_ccr = (int32_t)((T1_FREQ_148*sqrt(A_SQ/accel))/10); //C0值(69000*sqrt(6283/25))/10
max_lim = (uint32_t)(speed*speed/(A_x200*accel/10)); //计算到达最大速度所需步长
if(max_lim == 0) //如果除法计算结果为小数则使其为1
{
max_lim = 1;
}
accel_lim = (uint32_t)(step*decel/(accel+decel)); //加速到与减速段初速度相等时所需的步长(不一定是当step_ccr = min_ccr时)
if(accel_lim == 0) //如果除法计算结果为小数则使其为1
{
accel_lim = 1;
}
if(accel_lim <= max_lim) //如果加速度步长还没达到accel_lim但就需要开始减速
{
srd.decel_val = accel_lim - step; //此时的step=accel_lim+decel_lim设置减速段的步数(注意这里为负值)
}
else
{
srd.decel_val = -(max_lim*accel/decel); //此时step=accel_lim+max_lim+decel_lim
}
if(srd.decel_val == 0) //如果除法计算结果为小数则使其为1
{
srd.decel_val = -1;
}
srd.decel_start = step + srd.decel_val; //计算开始减速时的步数(减速步数为负值)
if(srd.step_ccr <= srd.min_ccr) //如果一开始的C0比匀速时的C还要大,则直接进入匀速状态,无需加速
{
srd.step_ccr = srd.min_ccr;
srd.run_state = RUN;
}
else
{
srd.run_state = ACCEL;
}
srd.accel_count = 0; //记录复位时的加减速值
}
tim_count = __HAL_TIM_GET_COUNTER(&htim3);
__HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_2,tim_count+srd.step_ccr/2); //每翻转两次才发送一次脉冲
HAL_TIM_OC_Start_IT(&htim3,TIM_CHANNEL_2); //打开定时器通道
}
梯形加减速中断服务
void HAL_TIM_OC_DelayElapsedCallback(TIM_HandleTypeDef *htim)
{
__IO uint32_t tim_count = 0;
__IO uint32_t tmp = 0;
uint16_t new_step_ccr = 0; //记录下一个脉冲的c值
__IO static uint32_t step_count = 0; //移动步数计数值
__IO static int32_t rest = 0; //用于记录余数,提高下一次计算的精度
__IO static uint8_t k = 0; //定时器需要翻转两次即进入两次中断才能产生一个脉冲输出
__IO static uint16_t last_accel_delay = 0; //加速状态下最后一次的C值
if(htim->Instance == TIM3)
{
tim_count = __HAL_TIM_GET_COUNTER(&htim3);
tmp = tim_count+srd.step_ccr/2;
__HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_2,tmp); //每翻转两次才发送一次脉冲
k++;
if(k == 2)
{
k = 0;
switch (srd.run_state)
{
case STOP:
En = 0;
step_count = 0; //清空计数值
rest = 0; //清空余数
g_motion_sta = 0; //电机为停止状态
HAL_TIM_OC_Stop_IT(&htim3,TIM_CHANNEL_2); //关闭比较输出通道
break;
case ACCEL:
step_count++; //步数加1
srd.accel_count++; //计数器值自加1
new_step_ccr = srd.step_ccr - (((2 *srd.step_ccr) + rest)/(4 * srd.accel_count + 1)); //计算下一次的c值
rest = ((2 * srd.step_ccr)+rest)%(4 * srd.accel_count + 1); //计算其余数
if(step_count >= srd.decel_start) //当计数值记到开始减速时
{
srd.accel_count = srd.decel_val; //加速计数值为减速计数值的初始值
srd.run_state = DECEL; //进入减速模式
}
else if(new_step_ccr <= srd.min_ccr) //当c值为匀速段c值时
{
last_accel_delay = new_step_ccr; //最后一次加速段c值等于匀速段c值
new_step_ccr = srd.min_ccr; //设置下一次c值为减速段c值
rest = 0; //余数为0
srd.run_state = RUN; //进入匀速模式
}
break;
case RUN:
step_count++; //步数加1
new_step_ccr = srd.min_ccr; //匀速状态下c值持续保持在最小
if(step_count >= srd.decel_start) //需要开始减速
{
srd.accel_count = srd.decel_val; //将减速段步数赋值给计步器
new_step_ccr = last_accel_delay; //将下一次c值赋值为最后一次加速段的c值
srd.run_state = DECEL; //进入减速模式
}
break;
case DECEL:
step_count++; //步数加1
srd.accel_count++; //计步器加1(这里体现为自减)
new_step_ccr = srd.step_ccr - (((2 * srd.step_ccr) + rest)/(4 * srd.accel_count + 1)); //计算下一次c值
rest = ((2 * srd.step_ccr)+rest)%(4 * srd.accel_count + 1); //计算余数
if(srd.accel_count >= 0) //检测是否为最后一步
{
srd.run_state = STOP;
}
break;
}
srd.step_ccr = new_step_ccr; //为下一个总步长赋值
}
}
}
Main函数
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();
/* USER CODE BEGIN 2 */
Stepper_Create_param(SPR*g_step_angle, g_step_accel, g_step_decel, g_set_speed);
/* USER CODE END 2 */
/* Infinite loop */
/* USER CODE BEGIN WHILE */
while (1)
{
/* USER CODE END WHILE */
/* USER CODE BEGIN 3 */
Motor_Start(En,1);
}
/* USER CODE END 3 */
}
重点逻辑梳理
这里梳理一些我认为的重点需要掌握的部分,同时也是为了我自己更好地复盘
其他的内容就不再赘述了,注释我都是尽可能标出来的,如果不出意外就可以开始进行梯形加减速了,第一次听到减速声别提有多开心了,如果出现了电机来回转的话很大可能是脉冲频率的问题,这里调节下定时器的计数频率即可,或者检查下参数有没有设置出错,比如电机转一圈对应的脉冲数是多少,我一开始设置错误电机也会来回不停地转,接下来计划是开始学习S型加减速