步进电机插补算法stm32文档格式.docx
《步进电机插补算法stm32文档格式.docx》由会员分享，可在线阅读，更多相关《步进电机插补算法stm32文档格式.docx（20页珍藏版）》请在冰豆网上搜索。
floatX,Y;
//实际运行的坐标
floatDelta1,Delta2,Delta3,Delta4;
//三种方法的误差，4为不走最后一步的误差
floatDelta;
//实际误差
//floatDeltaMax;
//最大误差
charway;
//选择的走法
inta;
//TIM6中断次数
intnum=0;
//总步数
inttx=1,ty=1;
//用来判断中断是否发生
intnumx,numy;
//计ＸＹ的步数
intcounter=0;
//计数值
floattime;
//时间
floatnxd,nyd;
//开始减速坐标
floatnx,ny;
//nx=Nx-0.0144f;
floatfenmu;
//公式中分母
//intfrex[20]={2,10,30,60,100,150,220,300,390,500};
//intfrey[20]={2,10,30,60,100,150,220,300,390,500};
intfrex[20]={1098,2931,7585,18242,37754,62245,81757,92414,97068,98901};
intfrey[20]={1098,2931,7585,18242,37754,62245,81757,92414,97068,98901};
intprex[20]={0};
intprey[20]={0};
intmain（void）
{
inti,j;
intnpwm=10;
//加减速每段脉冲数
//算出预分频值
for（i=0;
i<
10;
i++）
{
prex[i]=72000000/（frex[i]*200）;
prey[i]=72000000/（frey[i]*200）;
}
//配置，初始化
RCC_Configuration（）;
GPIO_Configuration（）;
USART_Configuration（）;
TIM_Configuration（）;
NVIC_Configuration（）;
Delay_Init（）;
TIM_Cmd（TIM2,ENABLE）;
//catchXY
TIM_Cmd（TIM6,ENABLE）;
//计时
//TIM_SetCounter（TIM6,0）;
//开始运行
//计算相关量
X=Mx;
Y=My;
k=（Ny-My）/（Nx-Mx）;
b=My-k*Mx;
//y=kx+b
nxd=Nx-90*0.0144f;
nyd=Ny-90*0.0144f;
nx=Nx-0.0144f;
ny=Ny-0.0144f;
fenmu=sqrt（k*k+1）;
for（i=1;
i++）//加速阶段，分10段
while（j<
npwm）
{
X1=X+0.0144f;
Y1=Y;
X2=X;
Y2=Y+0.0144f;
X3=X+0.0144f;
Y3=Y+0.0144f;
Delta1=fabs（k*X1-Y1+b）/fenmu;
Delta2=fabs（k*X2-Y2+b）/fenmu;
Delta3=fabs（k*X3-Y3+b）/fenmu;
//三种走法的误差
//选择最小误差走法
if（Delta1<
Delta2）
{way=1;
Delta=Delta1;
}
else
{way=2;
Delta=Delta2;
if（Delta3<
Delta）
{way=3;
Delta=Delta3;
switch（way）//实际走法，while（）为等待中断发生（中断失能），以确保一步一脉冲
case1:
X=X+0.0144f;
TIM_Cmd（TIM3,ENABLE）;
while（tx）;
tx=1;
break;
case2:
Y=Y+0.0144f;
TIM_Cmd（TIM4,ENABLE）;
while（ty）;
ty=1;
case3:
TIM_Cmd（TIM4,ENABLE）;
while（tx||ty）;
tx=1;
ty=1;
break;
num++;
j++;
TIM_PrescalerConfig（TIM3,prex[i]-1,TIM_PSCReloadMode_Immediate）;
//改预分频，频率
TIM_PrescalerConfig（TIM4,prey[i]-1,TIM_PSCReloadMode_Immediate）;
j=0;
while（X<
nxd||Y<
nyd）//最高频率阶段
switch（way）
//if（Delta>
DeltaMax）
//DeltaMax=Delta;
for（i=8;
i>
=0;
i--）//减速阶段，分10段
nx||Y<
ny）//最低频率阶段
//判断是否要走超出终点
Delta1=sqrt（（Nx-X1）*（Nx-X1）+（Ny-Y1）*（Ny-Y1））;
Delta2=sqrt（（Nx-X2）*（Nx-X2）+（Ny-Y2）*（Ny-Y2））;
Delta3=sqrt（（Nx-X3）*（Nx-X3）+（Ny-Y3）*（Ny-Y3））;
Delta4=sqrt（（Nx-X）*（Nx-X）+（Ny-Y）*（Ny-Y））;
if（Delta<
Delta4）
X=X+0.0144f;
TIM_Cmd（TIM3,ENABLE）;
while（tx==1）;
Y=Y+0.0144f;
TIM_Cmd（TIM4,ENABLE）;
while（ty==1）;
TIM_Cmd（TIM4,ENABLE）;
while（tx==1&
&
ty==1）;
counter=TIM_GetCounter（TIM6）;
TIM_Cmd（TIM6,DISABLE）;
time=a*65536/72.0f+counter/72.0f;
//us
//delay_ms（500）;
printf（"
总长度：
%fmm\n"
sqrt（（Nx-Mx）*（Nx-Mx）+（Ny-My）*（Ny-My）））;
总时间：
%fus\n"
time）;
总步数=%d\n"
num）;
//printf（"
numx=%d\n"
numx）;
numy=%d\n\n"
numy）;
while
（1）
{}
voidRCC_Configuration（void）
SystemInit（）;
RCC_APB1PeriphClockCmd（RCC_APB1Periph_TIM2|RCC_APB1Periph_TIM3|RCC_APB1Periph_TIM4
|RCC_APB1Periph_TIM6,ENABLE）;
RCC_APB2PeriphClockCmd（RCC_APB2Periph_GPIOA|RCC_APB2Periph_GPIOB,ENABLE）;
RCC_APB2PeriphClockCmd（RCC_APB2Periph_USART1,ENABLE）;
RCC_APB2PeriphClockCmd（RCC_APB2Periph_AFIO,ENABLE）;
voidGPIO_Configuration（void）
GPIO_InitTypeDefGPIO_InitStructure;
GPIO_InitStructure.GPIO_Pin=GPIO_Pin_7;
GPIO_InitStructure.GPIO_Mode=GPIO_Mode_AF_PP;
GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;
GPIO_Init（GPIOB,&
GPIO_InitStructure）;
GPIO_InitStructure.GPIO_Pin=GPIO_Pin_7|GPIO_Pin_9;
GPIO_Init（GPIOA,&
GPIO_InitStructure.GPIO_Pin=GPIO_Pin_2|GPIO_Pin_3|GPIO_Pin_10;
GPIO_InitStructure.GPIO_Mode=GPIO_Mode_IN_FLOATING;
voidTIM_Configuration（）
TIM_TimeBaseInitTypeDefTIM_TimeBaseStructure;
TIM_OCInitTypeDefTIM_OCInitStructure;
TIM_ICInitTypeDefTIM_ICInitStructure;
TIM_TimeBaseStructure.TIM_Prescaler=0;
TIM_TimeBaseStructure.TIM_ClockDivision=0;
TIM_TimeBaseStructure.TIM_CounterMode=TIM_CounterMode_Up;
TIM_TimeBaseStructure.TIM_Period=65536-1;
TIM_TimeBaseInit（TIM6,&
TIM_TimeBaseStructure）;
TIM_TimeBaseStructure.TIM_Prescaler=prex[0]-1;
//X
TIM_TimeBaseStructure.TIM_Period=200-1;
TIM_TimeBaseInit（TIM4,&
TIM_OCInitStructure.TIM_OCMode=TIM_OCMode_PWM1;
TIM_OCInitStructure.TIM_OutputState=TIM_OutputState_Enable;
TIM_OCInitStructure.TIM_Pulse=100-1;
TIM_OCInitStructure.TIM_OCPolarity=TIM_OCPolarity_Low;
TIM_OC2Init（TIM4,&
TIM_OCInitStructure）;
TIM_CtrlPWMOutputs（TIM4,ENABLE）;
TIM_TimeBaseStructure.TIM_Prescaler=prey[0]-1;
//Y
TIM_TimeBaseInit（TIM3,&
TIM_OC2Init（TIM3,&
TIM_CtrlPWMOutputs（TIM3,ENABLE）;
TIM_DeInit（TIM2）;
TIM_ICStructInit（&
TIM_ICInitStructure）;
TIM_ICInitStructure.TIM_Channel=TIM_Channel_3;
TIM_ICInitStructure.TIM_ICPolarity=TIM_ICPolarity_Rising;
TIM_ICInitStructure.TIM_ICSelection=TIM_ICSelection_DirectTI;
TIM_ICInitStructure.TIM_ICPrescaler=TIM_ICPSC_DIV1;
TIM_ICInitStructure.TIM_ICFilter=0x0;
TIM_ICInit（TIM2,&
TIM_ICInitStructure.TIM_Channel=TIM_Channel_4;
voidNVIC_Configuration（void）
NVIC_InitTypeDefNVIC_InitStructure;
NVIC_PriorityGroupConfig（NVIC_PriorityGroup_2）;
NVIC_InitStructure.NVIC_IRQChannel=TIM6_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=0;
NVIC_InitStructure.NVIC_IRQChannelSubPriority=2;
NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE;
NVIC_Init（&
NVIC_InitStructure）;
NVIC_InitStructure.NVIC_IRQChannel=TIM2_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=1;
TIM_ITConfig（TIM2,TIM_IT_CC3,ENABLE）;
TIM_ITConfig（TIM2,TIM_IT_CC4,ENABLE）;
TIM_ITConfig（TIM6,TIM_FLAG_Update,ENABLE）;
voidUSART_Configuration（void）
USART_InitTypeDefUSART_InitStructure;
USART_InitStructure.USART_BaudRate=115200;
USART_InitStructure.USART_WordLength=USART_WordLength_8b;
USART_InitStructure.USART_StopBits=USART_StopBits_1;
USART_InitStructure.USART_Parity=USART_Parity_No;
USART_InitStructure.USART_HardwareFlowControl=
USART_HardwareFlowControl_None;
USART_InitStructure.USART_Mode=USART_Mode_Tx|USART_Mode_Rx;
USART_Init（USART1,&
USART_InitStructure）;
USART_Cmd（USART1,ENABLE）;
intfputc（intch,FILE*f）
if（ch=='
\n'
）
while（USART_GetFlagStatus（USART1,USART_FLAG_TC）==RESET）;
USART_SendData（USART1,'
\r'
）;
USART_SendData（USART1,ch）;
returnch;
中断函数程序：
/**
**************************************************************************
升级会员 