电机控制7---步进电机模型及控制(4)

四、代码

以下代码是根据 AVR446 Linear speed control of stepper motor.pdf 所述控制方式编写而成，在STM32F103VET6+TB6600+42步步进电机上实验效果很好。

代码分三个文档。

1.Parameter.h 文件

#ifndef __PARAMETER_H
#define __PARAMETER_H

/*#define ARR 10
#define PSC 84
#define LENGTH 100
#define FRE_MAX 60000
#define FRE_MIN 1000
#define FLEXIBLE 8*/
int ARR=1000;
int PSC=8;
float FRE_MAX=400;//小于或等于300启动容易卡死。最佳选取400
float FRE_MIN=10;
int FLEXIBLE=32;

#define LENGTH 100
#define F_FLEX (200*FLEXIBLE)
#define PI 3.1415926
#define LENGTHD 1000
#define KESAI 1 
#endif

2.Stepper_Motor_Driver.h 文件

#ifndef __STEPPER_MOTOR_DRIVER_H
#define __STEPPER_MOTOR_DRIVER_H
#include "stm32f4xx_tim.h"
#include "misc.h"
#include "stm32f4xx_rcc.h"
#include "stm32f4xx_gpio.h"
#include "math.h"
#include "sys.h"

#define ARRAY_LENGTH 100
//定时器频率
#define F_t 2100000
//电机运行状态
#define STOP 0
#define ACCEL 1
#define RUN 2
#define DECEL 3
//运行方向
#define LEFT 1
#define RIGHT 0
//一圈步数
#define SPR 6400
#define ALPHA (2*3.14159/SPR) //步距角
#define A_T_x100 ((long)(ALPHA*F_t*100))     // 用于计算最大速度时的匹配值 步距角*脉冲数*100 
#define T1_FREQ_148 ((int)((F_t*0.676)/100)) // 用于计算首脉冲 C0

/**结构体 StepperMotorPara
  *摘要 步进电机所需要参数
  *参数 fre[100] 存储S型曲线频率
	      period[100] 存储S型曲线周期
				pulse 脉冲个数
				point 计数器
  */
typedef struct 
{
		unsigned int pulse;
		int dir;
		int en;
	  float step_delay;
	  float min_delay;
	 unsigned int decel_val;
	 unsigned int decel_start;
	          int run_state;
						int accel_count;
}StepperMotorPara;

void TIM4_Configuration(int arr,int psc);
void Moter_GPIO_Configuration(void);
void TIM4_IRQHandler(void);
void Motor_Init(void);
void JdugeDir(void);
void Speed_Parameter(int steps,unsigned int accel,unsigned int decel,unsigned int speeds);
void OneStep(void);
void Set_Speeds(int speeds,double angle);
void SwingUp(float adc);
#endif

3. Stepper_Motor_Driver.c 文件

#include "Stepper_Motor_Driver.h"
#include "Parameter.h"
#include "led.h" 
#include "delay.h"
#include "Monitor.h"
/*
	
*/
StepperMotorPara  MotorPara;
StepperMotorPara* PMotorPara;
unsigned int step_count = 0;
unsigned int new_step_delay;  //新的匹配值
unsigned int rest = 0;		//余数
extern  int PLUSE;
int n=1;
float delta;
int S=0;
/**函数 Motor_Init
  *摘要 初始化变量、TIM4、GPIOD、S型曲线。
  *参数 无
  *返回值 无
  */
void Motor_Init(void)
{
	 PMotorPara=&MotorPara;
	PMotorPara->pulse=60100;
	delta=0.005/FLEXIBLE;
	Moter_GPIO_Configuration();
	TIM4_Configuration(ARR-1,PSC-1);
	//Speed_Parameter(1000,5,5,55);
}
/**函数 Speed_Parameter
  *摘要 T型曲线参数计算
  *参数 steps:运行步数
				accel:加速度
				length:取点总数
				decel：减速带
				speeds：速度
  *返回值 无
  */
void Speed_Parameter(int steps,unsigned int accel,unsigned int decel,unsigned int speeds)
{
		unsigned long max_s_lim;
		unsigned int accel_lim;
	  PMotorPara->step_delay=F_t*sqrt(2*ALPHA/accel); //计算c0
	  PMotorPara->min_delay=ALPHA*F_t/speeds; //计算最大速度
	  max_s_lim=((long)(speeds*speeds))/((long)(2*ALPHA*accel));
	  /*0的情况*/
		if(max_s_lim == 0){
      max_s_lim = 1;
    }
		accel_lim = ((long)steps*decel) / (accel+decel);//加速度段n1
		if(accel_lim == 0){
      accel_lim = 1;
    }
		// 可以达到最大速度
    if(accel_lim <= max_s_lim){
      PMotorPara->decel_val = accel_lim - steps;//无法达到最大速度的减速步数
    }
    else{
      PMotorPara->decel_val = -((long)max_s_lim*accel)/decel;//能达到最大速度的减速步数
    }
		//0的情况 必须从减速到停止
    if(PMotorPara->decel_val == 0){
      PMotorPara->decel_val = -1;
    }
 
    // 找到何时开始减速的步数
    PMotorPara->decel_start = steps + PMotorPara->decel_val;
	//速度太低了，直接运行
    if(PMotorPara->step_delay <= PMotorPara->min_delay){
      PMotorPara->step_delay = PMotorPara->min_delay;
      PMotorPara->run_state = RUN;
    }
    else{
      PMotorPara->run_state = ACCEL; //否则从加速开始运行
    }
}


/**函数 TIM4_Configuration
	*摘要 TIM4配置，设置为比较输出模式。
	*参数 arr:下一个更新事件，自动重载寄存器周期值.
				psc:预分频值
  *返回值 无
  */
void TIM4_Configuration(int arr,int psc)
{
	  TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
		TIM_OCInitTypeDef  TIM_OCInitStructure;
		NVIC_InitTypeDef NVIC_InitStructure;
	//设置APB1，接通TIM4。
		RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4,ENABLE);
	//配置TIM4参数
	TIM_TimeBaseInitStructure.TIM_Period=arr;
	TIM_TimeBaseInitStructure.TIM_Prescaler=psc;
	TIM_TimeBaseInitStructure.TIM_CounterMode=TIM_CounterMode_Up;
	TIM_TimeBaseInitStructure.TIM_ClockDivision=TIM_CKD_DIV1;
	//初始化TIM4
	TIM_TimeBaseInit(TIM4,&TIM_TimeBaseInitStructure);
	//配置TIM4通道4为比较输出模式
	TIM_OCInitStructure.TIM_OCMode=TIM_OCMode_Toggle;//翻转模式
	TIM_OCInitStructure.TIM_Pulse=400;
	TIM_OCInitStructure.TIM_OutputState=TIM_OutputState_Enable;//比较输出使能
	TIM_OCInitStructure.TIM_OCPolarity=TIM_OCPolarity_Low;
	TIM_OCInitStructure.TIM_OCIdleState=TIM_OCIdleState_Set;
	TIM_OCInitStructure.TIM_OCNIdleState=TIM_OCIdleState_Reset;
	//初始化比较输出模式
	TIM_OC4Init(TIM4,&TIM_OCInitStructure);
	//关闭TIM4 CCR4预重载寄存器
	TIM_OC4PreloadConfig(TIM4,TIM_OCPreload_Disable);
	//TIM_ARRPreloadConfig(TIM4,ENABLE);
	//开TIM4中断
	TIM_ITConfig(TIM4,TIM_IT_CC4,ENABLE);
	//TIM4优先级配置
	NVIC_InitStructure.NVIC_IRQChannel=TIM4_IRQn;
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=0x00;
	NVIC_InitStructure.NVIC_IRQChannelSubPriority=0x00;
	NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE;
	NVIC_Init(&NVIC_InitStructure);
	//使能TIM4
	TIM_Cmd(TIM4,ENABLE);
	
}

/**函数 GPIO_Configuration
  *摘要 I/O口配置
  *参数 无
  *返回值 无
  */
void Moter_GPIO_Configuration(void)
{
		GPIO_InitTypeDef  GPIO_InitStructure;
	  //设置AHB1，接通GPIOD。
	  RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD,ENABLE);
	  //配置GPIOD|GPIO_Pin_10|GPIO_Pin_11
		GPIO_InitStructure.GPIO_Pin=GPIO_Pin_10|GPIO_Pin_11;//EN+/DIR+
		GPIO_InitStructure.GPIO_Mode=GPIO_Mode_OUT;
		GPIO_InitStructure.GPIO_OType=GPIO_OType_PP;
		GPIO_InitStructure.GPIO_Speed=GPIO_Speed_100MHz;
		GPIO_InitStructure.GPIO_PuPd=GPIO_PuPd_UP;
		//GPIOD|GPIO_Pin_10|GPIO_Pin_11初始化
		GPIO_Init(GPIOD,&GPIO_InitStructure);
		//复用模式
		//GPIO_Pin_15
		GPIO_PinAFConfig(GPIOD,GPIO_PinSource15,GPIO_AF_TIM4);
		GPIO_InitStructure.GPIO_Pin=GPIO_Pin_15;//PLUSE+
		GPIO_InitStructure.GPIO_Mode=GPIO_Mode_AF;
		GPIO_InitStructure.GPIO_OType=GPIO_OType_PP;
		GPIO_InitStructure.GPIO_Speed=GPIO_Speed_100MHz;
		GPIO_InitStructure.GPIO_PuPd=GPIO_PuPd_UP;
		//GPIOD|GPIO_Pin_15初始化
		GPIO_Init(GPIOD,&GPIO_InitStructure);
		//GPIOD|GPIO_Pin_11电平设置
		GPIO_SetBits(GPIOD,GPIO_Pin_11);//拉低，反转---负向
		GPIO_ResetBits(GPIOD,GPIO_Pin_10);//拉低，使能
		PMotorPara->dir=1;
		PMotorPara->en=0;
		
}
/**函数 SwingUp
*摘要 倒立摆起摆，周期性运动，直到摆杆达到设定值时，使用PID控制器稳定倒立摆。
  *参数 无
  *返回值 无
  */
void SwingUp(float speeds)
{
		
	  if(speeds>0.47)
		{
			speeds=0.96-0.47;
		}
		 speeds=10.0/speeds;
	  if(PMotorPara->pulse>=80000)//判断是否向右侧运动
			{  
				TIM_Cmd(TIM4,DISABLE);//失能时钟
				 PMotorPara->run_state = ACCEL;//加速状态
				 PMotorPara->accel_count=0;
				delay_ms(1);//延时
				PMotorPara->dir=RIGHT;//换向----向右侧运动
				GPIO_ResetBits(GPIOD,GPIO_Pin_11);//拉低端口
				step_count=0;//步数归零
				new_step_delay=0;//匹配值归零
				rest=0;//余数为零
				Speed_Parameter(1000,35,35,speeds);//设定新速度
				TIM_Cmd(TIM4,ENABLE);//使能时钟
			}
			else if(PMotorPara->pulse<60000)//判断是否向左侧运动
			{
				TIM_Cmd(TIM4,DISABLE);//失能时钟
				 PMotorPara->run_state = ACCEL;//加速状态
				 PMotorPara->accel_count=0;
				delay_ms(1);//延时
				PMotorPara->dir=LEFT;//换向----向左侧运动
				GPIO_SetBits(GPIOD,GPIO_Pin_11);//拉高端口
				step_count=0;//步数归零
				new_step_delay=0;//匹配值归零
				rest=0;//余数为零
				Speed_Parameter(1000,35,35,speeds);//设定新速度
				TIM_Cmd(TIM4,ENABLE);//使能时钟
			}
}
/**函数 JdugeDir
*摘要 判断当前距离，决定是否换向。
  *参数 无
  *返回值 无
  */
void JdugeDir(void)
{
			 //计算当前距离,并判断是否改变方向。
			if(PMotorPara->pulse>=120000)//判断是否向右侧运动
			{  TIM_Cmd(TIM4,DISABLE);//失能时钟
				 PMotorPara->run_state = ACCEL;//加速状态
				 PMotorPara->accel_count=0;
				delay_ms(1);//延时
				PMotorPara->dir=RIGHT;//换向----向右侧运动
				GPIO_ResetBits(GPIOD,GPIO_Pin_11);//拉低端口
				step_count=0;//步数归零
				new_step_delay=0;//匹配值归零
				rest=0;//余数为零
				Speed_Parameter(1000,5,5,25);//设定新速度
				TIM_Cmd(TIM4,ENABLE);//使能时钟
			}
			else if(PMotorPara->pulse<100)//判断是否向左侧运动
			{
				TIM_Cmd(TIM4,DISABLE);//失能时钟
				 PMotorPara->run_state = ACCEL;//加速状态
				 PMotorPara->accel_count=0;
				delay_ms(1);//延时
				PMotorPara->dir=LEFT;//换向----向左侧运动
				GPIO_SetBits(GPIOD,GPIO_Pin_11);//拉高端口
				step_count=0;//步数归零
				new_step_delay=0;//匹配值归零
				rest=0;//余数为零
				Speed_Parameter(1000,5,5,25);//设定新速度
				TIM_Cmd(TIM4,ENABLE);//使能时钟
			}
}
/**函数 Set_Speeds
*摘要 设置速度
  *参数 无
  *返回值 无
  */
void Set_Speeds(int speeds,double angle)
{      
     	PLUSE=(int)(533*(speeds*speeds)/35);
	if(angle<0)//判断是否向右侧运动
			{  TIM_Cmd(TIM4,DISABLE);//失能时钟
				 PMotorPara->run_state = ACCEL;//加速状态
				 PMotorPara->accel_count=0;
				delay_ms(1);//延时
				PMotorPara->dir=RIGHT;//换向----向右侧运动
				GPIO_ResetBits(GPIOD,GPIO_Pin_11);//拉低端口
				step_count=0;//步数归零
				new_step_delay=0;//匹配值归零
				rest=0;//余数为零
				Speed_Parameter(1000,45,45,speeds);//设定新速度
				TIM_Cmd(TIM4,ENABLE);//使能时钟
			}
			else //判断是否向左侧运动
			{
				TIM_Cmd(TIM4,DISABLE);//失能时钟
				 PMotorPara->run_state = ACCEL;//加速状态
				 PMotorPara->accel_count=0;
				delay_ms(1);//延时
				PMotorPara->dir=LEFT;//换向----向左侧运动
				GPIO_SetBits(GPIOD,GPIO_Pin_11);//拉高端口
				step_count=0;//步数归零
				new_step_delay=0;//匹配值归零
				rest=0;//余数为零
				Speed_Parameter(1000,45,45,speeds);//设定新速度
				TIM_Cmd(TIM4,ENABLE);//使能时钟
			}
}
/**函数 OneStep
*摘要 运行一步
  *参数 无
  *返回值 无
  */
void OneStep(void)
{
	TIM4->ARR=PMotorPara->step_delay;
	TIM_SetCompare4(TIM4,PMotorPara->step_delay);
}

void TIM4_IRQHandler(void)
{   
//	unsigned int new_step_delay;  //新的匹配值
  static int last_accel_delay;	//
//	//static unsigned int step_count = 0;//步数计数
//  static unsigned int rest = 0;		//余数
	
		if(TIM_GetITStatus(TIM4,TIM_IT_CC4)!=RESET)//检查TIM4中断是否发生
		{
			TIM_ClearITPendingBit(TIM4, TIM_IT_CC4);
			switch(PMotorPara->run_state) {
    case STOP:
      
		{
			step_count = 0;
      rest = 0;
      break;
		}
    case ACCEL:
	  {
			OneStep();  //运行一步
      step_count++;
      PMotorPara->accel_count++;
      new_step_delay = PMotorPara->step_delay - (((2 * ((long)PMotorPara->step_delay)) + rest)/(4 * PMotorPara->accel_count + 1));//计算出新的匹配值
      rest = ((2 * ((long)PMotorPara->step_delay))+rest)%(4 * PMotorPara->accel_count + 1); //求出余数
      if(step_count >= PMotorPara->decel_start) {	//判断是否进入减速
        PMotorPara->accel_count = PMotorPara->decel_val;
        PMotorPara->run_state = DECEL;
      }
			
      else if(new_step_delay <= PMotorPara->min_delay) { //判断是否能进入最大速度
        last_accel_delay = new_step_delay;
        new_step_delay = PMotorPara->min_delay;
        rest = 0;
        PMotorPara->run_state = RUN;
      }
      break;
		}
    case RUN:
	  {
			OneStep();	//运行一步
      step_count++;
      new_step_delay = PMotorPara->min_delay;
      if(step_count >= PMotorPara->decel_start) { //判断是否要减速了
        PMotorPara->accel_count = PMotorPara->decel_val;
        new_step_delay = last_accel_delay;
        PMotorPara->run_state = DECEL;
      }
      break;
		}
    case DECEL:
	  {
			OneStep();
      step_count++;
      PMotorPara->accel_count++;
      new_step_delay = PMotorPara->step_delay - (((2 * ((long)PMotorPara->step_delay)) + rest)/(4 * PMotorPara->accel_count + 1));
      rest = ((2 * ((long)PMotorPara->step_delay))+rest)%(4 * PMotorPara->accel_count + 1);
      if(PMotorPara->accel_count >= 0){ //步数走完了吗？
        PMotorPara->run_state = STOP;
      }
      break;
		}
  }
  PMotorPara->step_delay = new_step_delay;  //获取新的一次的匹配值
  	if(PMotorPara->dir==LEFT)//向左侧运动，脉冲数增加。
		{
			PMotorPara->pulse++;
		}
		if(PMotorPara->dir==RIGHT)//向右侧运动，脉冲数减少。
		{
			PMotorPara->pulse--;
		}
		PLUSE--;
		}
		
}