步进电机插补算法stm32.docx

步进电机插补算法stm32.docx

《步进电机插补算法stm32.docx》由会员分享，可在线阅读，更多相关《步进电机插补算法stm32.docx（20页珍藏版）》请在冰豆网上搜索。

步进电机插补算法stm32

#include"stm32f10x.h"

#include"delay.h"

#include"misc.h"

#include

#include"stm32f10x_tim.h"

#include"stm32f10x_rcc.h"

#include"stm32f10x_usart.h"

#include

voidRCC_Configuration（void）;

voidGPIO_Configuration（void）;

voidNVIC_Configuration（void）;

voidTIM_Configuration（void）;

voidUSART_Configuration（void）;

intfputc（intch,FILE*f）;

intfgetc（FILE*f）;

floatMx=1.44f,My=2.88f;//起点

floatNx=10.0f,Ny=7.61f;//终点

floatX1,Y1;

floatX2,Y2;

floatX3,Y3;//三种方法走后的坐标

floatk;//斜率

floatb;//y=kx+b

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;i++）//加速阶段，分10段

{

while（j

{

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

{way=1;Delta=Delta1;}

else

{way=2;Delta=Delta2;}

if（Delta3

{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;break;

case3:

X=X+0.0144f;Y=Y+0.0144f;TIM_Cmd（TIM3,ENABLE）;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

{

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

{way=1;Delta=Delta1;}

else

{way=2;Delta=Delta2;}

if（Delta3

{way=3;Delta=Delta3;}

switch（way）

{

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;break;

case3:

X=X+0.0144f;Y=Y+0.0144f;TIM_Cmd（TIM3,ENABLE）;TIM_Cmd（TIM4,ENABLE）;while（tx||ty）;tx=1;ty=1;break;

}

num++;

//if（Delta>DeltaMax）

//DeltaMax=Delta;

}

for（i=8;i>=0;i--）//减速阶段，分10段

{

TIM_PrescalerConfig（TIM3,prex[i]-1,TIM_PSCReloadMode_Immediate）;//改预分频，频率

TIM_PrescalerConfig（TIM4,prey[i]-1,TIM_PSCReloadMode_Immediate）;

while（j

{

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

{way=1;Delta=Delta1;}

else

{way=2;Delta=Delta2;}

if（Delta3

{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;break;

case3:

X=X+0.0144f;Y=Y+0.0144f;TIM_Cmd（TIM3,ENABLE）;TIM_Cmd（TIM4,ENABLE）;while（tx||ty）;tx=1;ty=1;break;

}

num++;

j++;

}

j=0;

}

while（X

{

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

{way=1;Delta=Delta1;}

else

{way=2;Delta=Delta2;}

if（Delta3

{way=3;Delta=Delta3;}

switch（way）

{

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;break;

case3:

X=X+0.0144f;Y=Y+0.0144f;TIM_Cmd（TIM3,ENABLE）;TIM_Cmd（TIM4,ENABLE）;while（tx||ty）;tx=1;ty=1;break;

}

num++;

//if（Delta>DeltaMax）

//DeltaMax=Delta;

}

//判断是否要走超出终点

{

X1=X+0.0144f;

Y1=Y;

X2=X;

Y2=Y+0.0144f;

X3=X+0.0144f;

Y3=Y+0.0144f;

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））;

if（Delta1

{way=1;Delta=Delta1;}

else

{way=2;Delta=Delta2;}

if（Delta3

{way=3;Delta=Delta3;}

Delta4=sqrt（（Nx-X）*（Nx-X）+（Ny-Y）*（Ny-Y））;

if（Delta

{

switch（way）

{

case1:

X=X+0.0144f;TIM_Cmd（TIM3,ENABLE）;while（tx==1）;break;

case2:

Y=Y+0.0144f;TIM_Cmd（TIM4,ENABLE）;while（ty==1）;break;

case3:

X=X+0.0144f;Y=Y+0.0144f;TIM_Cmd（TIM3,ENABLE）;TIM_Cmd（TIM4,ENABLE）;while（tx==1&&ty==1）;break;

}

num++;

}

}

}

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）））;

printf（"总时间：

%fus\n",time）;

printf（"总步数=%d\n",num）;

//printf（"numx=%d\n",numx）;

//printf（"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_InitStructure.GPIO_Pin=GPIO_Pin_2|GPIO_Pin_3|GPIO_Pin_10;

GPIO_InitStructure.GPIO_Mode=GPIO_Mode_IN_FLOATING;

GPIO_Init（GPIOA,&GPIO_InitStructure）;

}

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_TimeBaseStructure）;

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_TimeBaseStructure.TIM_Period=200-1;

TIM_TimeBaseInit（TIM3,&TIM_TimeBaseStructure）;

TIM_OCInitStructure.TIM_Pulse=100-1;

TIM_OC2Init（TIM3,&TIM_OCInitStructure）;

TIM_CtrlPWMOutputs（TIM3,ENABLE）;

TIM_DeInit（TIM2）;

TIM_ICStructInit（&TIM_ICInitStructure）;

TIM_ICInitStructure.TIM_Channel=TIM_Channel_3;//X

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_ICInitStructure.TIM_Channel=TIM_Channel_4;//Y

TIM_ICInit（TIM2,&TIM_ICInitStructure）;

}

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;

NVIC_InitStructure.NVIC_IRQChannelSubPriority=2;

NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE;

NVIC_Init（&NVIC_InitStructure）;

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'）;

}

while（USART_GetFlagStatus（USART1,USART_FLAG_TC）==RESET）;

USART_SendData（USART1,ch）;

returnch;

}

中断函数程序：

/**

**************************************************************************