STM32步进伺服电机梯形加速源程序单轴简易运动控制器

ID:82014 · 发表于 2018-4-26 12:32

步进电机梯形加速程序

单片机源程序如下:

/*基于STM32的单轴简易运动控制器/脉冲发生器*/
/*脉冲+方向控制步进伺服电机*/
/*
优化记录:
增加急停GPIOC.0、正向极限GPIOC.1、负向极限GPIOC.2等输入IO接点
中断修改TIMx_PSC一个寄存器的值，而不是修改TIMx_ARR预加载寄存器+TIMx_CCRx比较值寄存器两个值，缩短中断处理时间
定位指令DRVI/DRVA中，目标频率设定过高、而实际输出脉冲数过少时，则不必加速到目标频率即进入减速区
*/
/*
DRVI(A)；相对定位，输出A(A取绝对值)个脉冲
A不能为0
若A为正数，则方向为正、GPIOB.0为高电平
若A为负数，则方向为负、GPIOB.0为低电平
DRVA(A) 绝对定位，输出脉冲，运行至A个脉冲的位置
若目标位置A等于当前位置D，则不执行脉冲输出
若A大于D 则方向为正GPIOB.5为高电平
若A小于D 则方向为负GPIOB.5为低电平
GPIOB.1为脉冲输出
GPIOB.0为方向输出
占空比为50%
GPIOC.0急停
GPIOC.1正向极限
GPIOC.2反向极限
GPIOC.3
GPIOC.4
GPIOC.5
GPIOC.6
GPIOC.7启动
阶梯曲线形式加减速
加减速时间以10毫秒为基本单位
加减速以每10毫秒为一级
例如
加减速时间为50毫秒，则加减速级数为50/10=5
加减速时间为100毫秒，则加减速级数为100/10=10
加减速时间为150毫秒，则加减速级数为150/10=15
*/
#include "sys.h"
#include "delay.h"
#define MasterFrequency 0x100000//最高频率限制100K
long Current; //当前位置脉冲数
long Target; //目标位置脉冲数
long StartSave; //定位指令刚开始启动时的当前值
long DownStartSave; //开始进入减速时的当前值
typedef enum
{
OFF = 0,
ON = 1,
}STATUS_Type;
typedef enum
{
us=1,
ms=1000,
sce=1000000,
}DELAY_Type;
STATUS_Type RunFlag; //定位指令脉冲输出执行标志
STATUS_Type StopCommand;//定位指令脉冲输出停止命令标志
STATUS_Type PlusMinus; //正负方向标志
u32 StartFreq; //启动频率
u32 TargetFreq; //目标频率
u32 UDTimer=1000; //加减速时间
u32 LadderFreq[102];
u16 LadderPSC[202]; //加减速0至9级速度/频率预分频值
u16 LadderNum; //加减速速度级数
u16 LadderOrderNum; //加减速速度编号
long LadderTarget[202]; //各速度等级目标值
int m;
void MyTimer3_Init()//定时器3初始化
{
RCC->APB2ENR|=(1<<3)|(1<<0); //使能AFIO、GPIOB时钟
GPIOB->CRL&=0xffffff00; //PB5
GPIOB->CRL|=0x000000a2; //配置PORTB.1为复用推挽输出、配置PORTB.0为推挽输出，输出最大频率2MHz 00B000a2
GPIOB->BRR=1<<0;
RCC->APB1ENR|=1<<1; //使能定时器TIMER3时钟
TIM3->CR1|=1<<2; //设置只有计数溢出作为T3更新中断
TIM3->DIER|=1<<0; //允许定时器3计数溢出中断
MY_NVIC_Init(1,3,TIM3_IRQn,2);
TIM3->CCMR2&=~(3<<8); //T3_CH4通道配置为输出模式
TIM3->CCMR2|=7<<12; //T3_CH4为PWM模式2
TIM3->CCER|=1<<12; //T3_CH4通道输出使能
TIM3->PSC=71;
}
void Pluse_start()
{
RunFlag=ON; //脉冲输出定位指令执行标志置ON
StartSave=Current;
LadderOrderNum=0;//加减速级数序号为0
TIM3->ARR=LadderPSC[0];
TIM3->CCR4=TIM3->ARR>>1; //匹配值1等于重装值一半，是以占空比为50%
//delay_ms(2); //脉冲信号比方向信号滞后，以提高可靠性
TIM3->CR1|=1<<0; //启动定时器TIMER2计数
}
/*********************************************************************************
函数名称：DRVI
函数功能：相对定位
入口参数：long offset相对偏移脉冲,u32 frequency最高频率
返回值：无
*********************************************************************************/
void DRVI(long offset,u32 frequency)
{
u16 h;
u16 i;
u32 j;
if((offset!=0)&&(RunFlag==OFF)&&((GPIOC->IDR&0x01)==0))//相对偏移值为0则不接受命令，脉冲输出已执行，不接受命令
{
Target=Current+offset; //目标值等于当前值加上相对偏移值
if(frequency<StartFreq) //如果设定目标频率小于启动频率
{
frequency=StartFreq;
}
else if(frequency>MasterFrequency)//否则如果设定目标频率高于最高限制频率
{
frequency=MasterFrequency;
}
LadderNum=UDTimer/10;//加减速级数
j=(frequency-StartFreq)/LadderNum;//等差
for(i=0;i<LadderNum;i++)
{
LadderFreq[i]=i*j+StartFreq;//加减速各阶梯频率
LadderPSC[i]=(6000000/LadderFreq[i])-1;//加减速各阶梯频率对应定时器预分频值
}
LadderFreq[LadderNum]=frequency;//目标频率，最高频率
LadderPSC[LadderNum]=6000000/frequency-1;//目标频率(最高频率)对应定时器预分频值
if(offset>0)//相对偏移值为正数
{
GPIOB->BSRR=1<<0;//相对偏移值为正数，方向为正，方向信号高电平
PlusMinus=ON;//正负方向标志置ON
LadderTarget[0]=Current+StartFreq/100;//加速第一段目标脉冲值
for(i=1;i<LadderNum;i++)
{
LadderTarget[i]=LadderTarget[i-1]+LadderFreq[i]/100;//加速各段目标脉冲值
}
while(offset<=((LadderTarget[LadderNum-1]-Current)<<1))//如果偏移量小于二倍加速增量
{
LadderNum--;//加速等级数减一频率设定过高、实际输出脉冲数过少的情况下不必加速至设定频率，避免过冲
}
for(i=0,h=LadderNum<<1; i<LadderNum; i++,h--)
{
LadderPSC[h]=LadderPSC[i];//减速各段定时器重装值
}
LadderTarget[LadderNum<<1]=Target;//减速最后一段目标脉冲值
for(i=(LadderNum<<1)-1,h=0;i>LadderNum;i--,h++)
{
LadderTarget[i]=LadderTarget[i+1]-LadderFreq[h]/100;//减速各段目标脉冲值
}
}
else//否则相对偏移值为负数
{
GPIOB->BRR=1<<0;//相对偏移值为负数，方向为负，方向信号低电平
PlusMinus=OFF;//正负方向标志OFF
LadderTarget[0]=Current-StartFreq/100;//加速第一段目标脉冲值
for(i=1;i<LadderNum;i++)
{
LadderTarget[i]=LadderTarget[i-1]-LadderFreq[i]/100;//加速各段目标脉冲值
}
while(offset>=((LadderTarget[LadderNum-1]-Current)<<1))//如果偏移量小于二倍加速增量
{
LadderNum--;//加速等级数减一频率设定过高、实际输出脉冲数过少的情况下不必加速至设定频率，避免过冲
}
for(i=0,h=LadderNum<<1; i<LadderNum; i++,h--)
{
LadderPSC[h]=LadderPSC[i];
}
LadderTarget[LadderNum<<1]=Target;//减速最后一段目标脉冲值
for(i=(LadderNum<<1)-1,h=0;i>LadderNum;i--,h++)
{
LadderTarget[i]=LadderTarget[i+1]+LadderFreq[h]/100;//减速各段目标脉冲值
}
}
LadderTarget[LadderNum]=Target + Current - LadderTarget[LadderNum-1];
Pluse_start();//脉冲输出正式启动
}
}
/*********************************************************************************
函数名称：DRVA
函数功能：绝对定位
入口参数：long target目标位置脉冲,u32 frequency最高频率
返回值：无
*********************************************************************************/
void DRVA(long target,u32 frequency)//3200 2khz
{
u16 h;
u16 i;
u32 j;
long offset=target-Current;//
if((offset!=0)&&(RunFlag==OFF)&&((GPIOC->IDR&0x01)==0)) //目标位置等于当前位置，则不接受命令
{
///////////////////////////////////////////////////////////////////////////////////////////////////////
Target=target; //目标位置设定(等于参数)
if(frequency<StartFreq) //如果设定目标频率小于启动频率
{
frequency=StartFreq;
}
else if(frequency>MasterFrequency)//否则如果设定目标频率高于最高限制频率
{
frequency=MasterFrequency;
}
LadderNum=UDTimer/10;//加减速级数分成100级
j=(frequency-StartFreq)/LadderNum;//等差每个阶段所加的频率数
for(i=0;i<LadderNum;i++) //获取每个阶段的速度值
{
LadderFreq[i]=i*j+StartFreq;//加减速各阶梯频率每个阶段的速度等级到J
LadderPSC[i]=(1000000/LadderFreq[i])-1;//加减速各阶梯频率对应定时器初值
}
LadderFreq[LadderNum]=frequency;
LadderPSC[LadderNum]=1000000/frequency-1;//
/////////////////////////////////////////////////////////////////////////////////////////////////////
if(offset>0)//目标位置值大于当前位置值
{
GPIOB->BSRR=1<<0;//则方向为正，方向信号高电平
PlusMinus=ON;//正负方向标志置ON
LadderTarget[0]=Current+StartFreq/100;
for(i=1;i<LadderNum;i++)
{
LadderTarget[i]=LadderTarget[i-1]+LadderFreq[i]/100;
}
while(offset<=((LadderTarget[LadderNum-1]-Current)<<1))//如果偏移量小于二倍加速增量
{
LadderNum--;//加速等级数减一频率设定过高、实际输出脉冲数过少的情况下不必加速至设定频率，避免过冲
}
for(i=0,h=LadderNum<<1; i<LadderNum; i++,h--)
{
LadderPSC[h]=LadderPSC[i];
}
LadderTarget[LadderNum<<1]=Target;//减速最后一段目标脉冲值
for(i=(LadderNum<<1)-1,h=0;i>LadderNum;i--,h++)
{
LadderTarget[i]=LadderTarget[i+1]-LadderFreq[h]/100;//减速各段目标脉冲值
}
}
else//否则目标位置值小于当前位置值，
{
GPIOB->BRR=1<<0;//则方向为负，方向信号低电平
PlusMinus=OFF;//正负方向标志OFF
LadderTarget[0]=Current-StartFreq/100;
for(i=1;i<LadderNum;i++)
{
LadderTarget[i]=LadderTarget[i-1]-LadderFreq[i]/100;
}
while(offset>=((LadderTarget[LadderNum-1]-Current)<<1))//如果偏移量小于二倍加速增量
{
LadderNum--;//加速等级数减一频率设定过高、实际输出脉冲数过少的情况下不必加速至设定频率，避免过冲
}
for(i=0,h=LadderNum<<1; i<LadderNum; i++,h--)
{
LadderPSC[h]=LadderPSC[i];
}
LadderTarget[LadderNum<<1]=Target;//减速最后一段目标脉冲值
for(i=(LadderNum<<1)-1,h=0; i>LadderNum; i--,h++)
{
LadderTarget[i]=LadderTarget[i+1]+LadderFreq[h]/100;//减速各段目标脉冲值
}
}
LadderTarget[LadderNum]=Target + Current - LadderTarget[LadderNum-1];//匀速段目标位置/进入减速时位置数组元素60
Pluse_start();//脉冲输出正式启动
}
}
/*********************************************************************************
函数名称：SLOPE
函数功能：斜坡设置(坡度斜率设置) 限定启动频率在100---1000之间限制时间在50---1000
入口参数：u32 frequency启动频率,u32 timer加减速时间
返回值：无
*********************************************************************************/
void SLOPE(u32 frequency,u32 timer)//200hz 600
{
if(frequency<100)
{
StartFreq=100;
}
else if(frequency>1000)
{
StartFreq=1000;
}
else
{
StartFreq=frequency;
}//启动频率设置大于等于100小于等于1000
if(timer<50)
{
UDTimer=50;
}
else if(timer>1000)
{
UDTimer=1000;
}
else
{
UDTimer=(timer/10)*10; //取整数
}//加减速时间设置大于等于50小于等于1000，且为整十数
}
void TIM3_IRQHandler() //定时器3全局中断函数
{
long temp;
if(TIM3->SR&0x0001)
{
if(PlusMinus==ON)
{
temp=Current;
//如果方向为正，当前值加一
temp++;
Current=temp;
}
else
{
temp=Current;
temp--; //否则方向为负，当前值减一
Current=temp;
}
if(Current==LadderTarget[LadderOrderNum])
{
if(LadderOrderNum< (LadderNum<<1))
{
LadderOrderNum++;
TIM3->PSC=LadderPSC[LadderOrderNum];
}
else
{
TIM3->CR1&=~(1<<0);
TIM3->CNT=0x0000;
RunFlag=OFF;
}
}
TIM3->SR=0x0000;
}
}
void Variable_Init()
{
Target=0;//目标位置脉冲值
Current=0;//当前位置脉冲值
RunFlag=OFF;//脉冲定位指令执行标志
StopCommand=OFF;//定位指令脉冲输出停止命令标志
}
void PAUSE()
{
while(RunFlag==ON);
}
int main(void)
{
// Stm32_Clock_Init(9);
//delay_init(72);
MyTimer3_Init(); //定时器2初始化
Variable_Init(); //变量初始化
SLOPE(200,600); //启动频率200Hz，加减速时间600毫秒斜率设置
DRVA(1200,4000);
while(1)
{
//以2KHz频率前进3200脉冲当量距离
// //PAUSE(); //等待脉冲输出执行完毕
// delay_ms(500);
// DRVA(0,20000); //以2KHz频率前进3200脉冲当量距离
// PAUSE(); //等待脉冲输出执行完毕
// delay_ms(500);
}
}