步進電機-插補算法stm32

1、#include "stm32f10x.h"#include "delay.h"#include "misc.h"#include <stdio.h>#include "stm32f10x_tim.h"#include "stm32f10x_rcc.h"#include "stm32f10x_usart.h"#include <math.h>void RCC_Configuration(void);void GPIO_Configuration(voi

2、d);void NVIC_Configuration(void);void TIM_Configuration(void);void USART_Configuration(void);int fputc(int ch,FILE *f);int fgetc(FILE *f);float Mx=1.44f,My=2.88f;/起點float Nx=10.0f,Ny=7.61f;/終點float X1,Y1;float X2,Y2;float X3,Y3;/三種方法走后的坐標float k;/斜率float b;/y=kx+bfloat X,Y;/實際運行的坐標float Delta1,Delta

3、2,Delta3,Delta4;/三種方法的誤差，4為不走最后一步的誤差float Delta;/實際誤差/ float DeltaMax;/最大誤差char way;/選擇的走法int a;/TIM6 中斷次數(shù)int num=0;/總步數(shù)int tx=1,ty=1;/用來判斷中斷是否發(fā)生int numx,numy;/計的步數(shù)int counter=0;/計數(shù)值float time;/時間 float nxd,nyd;/開始減速坐標 float nx,ny;/nx=Nx-0.0144f; float fenmu;/公式中分母 / int frex20=2,10,30,60,100,150,22

4、0,300,390,500;/ int frey20=2,10,30,60,100,150,220,300,390,500;int frex20=1098,2931,7585,18242,37754,62245,81757,92414,97068,98901;int frey20=1098,2931,7585,18242,37754,62245,81757,92414,97068,98901;int prex20=0;int prey20=0;int main(void)int i,j; int npwm=10; /加減速每段脈沖數(shù)/算出預(yù)分頻值for(i=0;i<10;i+)prexi

5、=72000000/(frexi*200);preyi=72000000/(freyi*200); /配置，初始化 RCC_Configuration(); GPIO_Configuration();USART_Configuration();TIM_Configuration();NVIC_Configuration(); Delay_Init();TIM_Cmd(TIM2, ENABLE);/catch X YTIM_Cmd(TIM6, ENABLE);/計時/ TIM_SetCounter(TIM6,0);/開始運行/計算相關(guān)量 X=Mx; Y=My;k=(Ny-My)/(Nx-Mx);

6、b=My-k*Mx;/y=kx+bnxd=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<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;/

7、三種走法的誤差 /選擇最小誤差走法 if(Delta1<Delta2) way=1;Delta=Delta1; else way=2;Delta=Delta2; if(Delta3<Delta) way=3;Delta=Delta3; switch (way)/實際走法，while()為等待中斷發(fā)生（中斷失能），以確保一步一脈沖 case 1: X=X+0.0144f;TIM_Cmd(TIM3,ENABLE); while(tx); tx=1; break; case 2: Y=Y+0.0144f;TIM_Cmd(TIM4,ENABLE); while(ty); ty=1; bre

8、ak; case 3: 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, prexi-1,TIM_PSCReloadMode_Immediate);/改預(yù)分頻，頻率TIM_PrescalerConfig(TIM4, preyi-1,TIM_PSCReloadMode_Immediate);j=0; while(X<nxd|Y<nyd) /最高頻率階段 X1=X+0.0

9、144f; 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) case 1: X=X+0.0144f;TIM_Cmd(TIM3,E

10、NABLE); while(tx); tx=1; break; case 2: Y=Y+0.0144f;TIM_Cmd(TIM4,ENABLE); while(ty); ty=1; break; case 3: 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(TI

11、M3, prexi-1,TIM_PSCReloadMode_Immediate);/改預(yù)分頻，頻率TIM_PrescalerConfig(TIM4, preyi-1,TIM_PSCReloadMode_Immediate);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;/三種走法的誤差 /選擇最小誤差走

12、法 if(Delta1<Delta2) way=1;Delta=Delta1; else way=2;Delta=Delta2; if(Delta3<Delta) way=3;Delta=Delta3; switch (way)/實際走法，while()為等待中斷發(fā)生（中斷失能），以確保一步一脈沖 case 1: X=X+0.0144f;TIM_Cmd(TIM3,ENABLE); while(tx); tx=1; break; case 2: Y=Y+0.0144f;TIM_Cmd(TIM4,ENABLE); while(ty); ty=1; break; case 3: X=X+

13、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<nx|Y<ny) /最低頻率階段 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(De

14、lta1<Delta2) way=1;Delta=Delta1; else way=2;Delta=Delta2; if(Delta3<Delta) way=3;Delta=Delta3; switch (way) case 1: X=X+0.0144f;TIM_Cmd(TIM3,ENABLE); while(tx); tx=1; break; case 2: Y=Y+0.0144f;TIM_Cmd(TIM4,ENABLE); while(ty); ty=1; break; case 3: X=X+0.0144f; Y=Y+0.0144f; TIM_Cmd(TIM3,ENABLE)

15、;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

16、-Y3); if(Delta1<Delta2) way=1;Delta=Delta1; else way=2;Delta=Delta2; if(Delta3<Delta) way=3;Delta=Delta3; Delta4=sqrt(Nx-X)*(Nx-X)+(Ny-Y)*(Ny-Y); if(Delta<Delta4) switch (way) case 1:X=X+0.0144f;TIM_Cmd(TIM3, ENABLE); while(tx=1); break; case 2:Y=Y+0.0144f;TIM_Cmd(TIM4, ENABLE);while(ty=1);

17、 break; case 3: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(" 總長度 ： %f mmn",sqrt(Nx-Mx)*(Nx-Mx)+(Ny-My)*(Ny-My);pr

18、intf(" 總時間 ： %f usn",time);printf("總步數(shù)=%dn",num);/ printf("numx=%dn",numx);/ printf("numy=%dnn",numy);while(1)void RCC_Configuration(void) SystemInit();RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2|RCC_APB1Periph_TIM3|RCC_APB1Periph_TIM4 |RCC_APB1Periph_TIM6,ENAB

19、LE); RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA|RCC_APB2Periph_GPIOB , ENABLE);RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1,ENABLE);RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO,ENABLE);void GPIO_Configuration(void)GPIO_InitTypeDef GPIO_InitStructure;GPIO_InitStructure.GPIO_Pin=GPIO_Pin_7;GPIO_InitStr

20、ucture.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_Mo

21、de=GPIO_Mode_IN_FLOATING;GPIO_Init(GPIOA,&GPIO_InitStructure);void TIM_Configuration()TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;TIM_OCInitTypeDef TIM_OCInitStructure;TIM_ICInitTypeDef TIM_ICInitStructure;TIM_TimeBaseStructure.TIM_Prescaler=0;TIM_TimeBaseStructure.TIM_ClockDivision=0;TIM_Time

22、BaseStructure.TIM_CounterMode=TIM_CounterMode_Up;TIM_TimeBaseStructure.TIM_Period=65536-1;TIM_TimeBaseInit(TIM6,&TIM_TimeBaseStructure);TIM_TimeBaseStructure.TIM_Prescaler=prex0-1;/XTIM_TimeBaseStructure.TIM_Period=200-1;TIM_TimeBaseInit(TIM4,&TIM_TimeBaseStructure);TIM_OCInitStructure.TIM_O

23、CMode=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=prey0-1;/YTIM_Time

24、BaseStructure.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

25、;/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 =

26、TIM_Channel_4;/Y TIM_ICInit(TIM2, &TIM_ICInitStructure);void NVIC_Configuration(void)NVIC_InitTypeDef NVIC_InitStructure;NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);NVIC_InitStructure.NVIC_IRQChannel = TIM6_IRQn; NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;NVIC_InitStructure.NVIC

27、_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_IRQChanne

28、lCmd = 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);void USART_Configuration(void) USART_InitTypeDef USART_InitStructure; USART_InitStructure.USART_BaudRate =115200; USART_InitStructu

29、re.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_In

30、it(USART1, &USART_InitStructure); USART_Cmd(USART1,ENABLE);int fputc(int ch,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); return ch;中斷函數(shù)程序：

31、/* * * file Project/Template/stm32f10x_it.c * author MCD Application Team * version V3.0.0 * date 04/06/2009 * brief Main Interrupt Service Routines. * This file provides template for all exceptions handler and * peripherals interrupt service routine. * * copy * * THE PRESENT FIRMWARE WHICH IS FOR G

32、UIDANCE ONLY AIMS AT PROVIDING CUSTOMERS * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING * FROM THE CONTENT OF SUCH FIR

33、MWARE AND/OR THE USE MADE BY CUSTOMERS OF THE * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. * * <h2><center>&copy; COPYRIGHT 2009 STMicroelectronics</center></h2> */ /* Includes -*/#include "stm32f10x_it.h"#include "stm32f10x_exti

34、.h"#include "stm32f10x_rcc.h"#include "misc.h"#include "stm32f10x_dma.h"#include "stm32f10x_usart.h"#include <stdio.h>/* addtogroup Template_Project * */* Private typedef -*/* Private define -*/* Private macro -*/* Private variables -*/* Private fun

35、ction prototypes -*/* Private functions -*/*/* Cortex-M3 Processor Exceptions Handlers */*/* * brief This function handles NMI exception. * param None * retval : None */void NMI_Handler(void)/* * brief This function handles Hard Fault exception. * param None * retval : None */void HardFault_Handler(

36、void) /* Go to infinite loop when Hard Fault exception occurs */ while (1) /* * brief This function handles Memory Manage exception. * param None * retval : None */void MemManage_Handler(void) /* Go to infinite loop when Memory Manage exception occurs */ while (1) /* * brief This function handles Bu

37、s Fault exception. * param None * retval : None */void BusFault_Handler(void) /* Go to infinite loop when Bus Fault exception occurs */ while (1) /* * brief This function handles Usage Fault exception. * param None * retval : None */void UsageFault_Handler(void) /* Go to infinite loop when Usage Fau

38、lt exception occurs */ while (1) /* * brief This function handles SVCall exception. * param None * retval : None */void SVC_Handler(void)/* * brief This function handles Debug Monitor exception. * param None * retval : None */void DebugMon_Handler(void)/* * brief This function handles PendSVC exception. * param None * retval : None */void PendSV_Handler(void)/