电机控制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
#endif2.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--;
}
}
电动机视频汇总:电动机视频
其它博文链接: 电机控制1-电机
电机控制4---步进电机模型及控制(1)
电机控制5---步进电机模型及控制(2)
电机控制6---步进电机模型及控制(3)
电机控制7---步进电机模型及控制(4)
电机控制8---步进电机常见问题