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);
}
}

復制代碼

所有資料51hei提供下載:

步進電機寄存器版本.zip (698.35 KB, 下載次數: 187)

ID:445569 · 發表于 2019-2-2 11:08

學術型

ID:138247 · 發表于 2019-9-9 11:34

感謝分享，非常有幫助。。

ID:245179 · 發表于 2019-10-21 16:56

寫的不錯，已分享使用

ID:334838 · 發表于 2019-10-24 15:11

學習下，正好需要做電機控制

ID:626923 · 發表于 2019-10-28 22:31

學習下

ID:579837 · 發表于 2019-12-18 00:01

最近正在學習電機控制，謝謝分享

ID:236035 · 發表于 2019-12-20 10:51

謝謝樓主分享，這是個弱項，還沒搞過項目。

ID:353855 · 發表于 2020-2-9 18:19

感謝分享，非常有幫助。。

ID:353855 · 發表于 2020-2-18 16:21

寫的不錯，已分享使用

ID:25103 · 發表于 2020-7-16 12:12

仿真好像不能用，請教樓主，謝謝！

帳號		自動登錄	找回密碼
密碼			立即注冊

久久久久久久999_99精品久久精品一区二区爱城_成人欧美一区二区三区在线播放_国产精品日本一区二区不卡视频_国产午夜视频_欧美精品在线观看免费

STM32步進伺服電機梯形加速源程序 單軸簡易運動控制器

STM32步進伺服電機梯形加速源程序單軸簡易運動控制器