完成S型加减速

This commit is contained in:
MQjehovah 2018-03-02 17:32:45 +08:00
parent 67382ed910
commit a2545f3122
8 changed files with 233 additions and 115 deletions

View File

@ -124,7 +124,7 @@
</OPTHX>
<Simulator>
<UseSimulator>0</UseSimulator>
<LoadApplicationAtStartup>1</LoadApplicationAtStartup>
<LoadApplicationAtStartup>0</LoadApplicationAtStartup>
<RunToMain>1</RunToMain>
<RestoreBreakpoints>1</RestoreBreakpoints>
<RestoreWatchpoints>1</RestoreWatchpoints>
@ -359,7 +359,7 @@
<wLevel>0</wLevel>
<uThumb>0</uThumb>
<uSurpInc>0</uSurpInc>
<uC99>0</uC99>
<uC99>1</uC99>
<useXO>0</useXO>
<VariousControls>
<MiscControls></MiscControls>
@ -428,16 +428,36 @@
<FileType>1</FileType>
<FilePath>..\..\src\Driver\delay.c</FilePath>
</File>
<File>
<FileName>usart_debug.c</FileName>
<FileType>1</FileType>
<FilePath>..\..\src\Driver\usart_debug.c</FilePath>
</File>
<File>
<FileName>stepper.c</FileName>
<FileType>1</FileType>
<FilePath>..\..\src\Driver\stepper.c</FilePath>
</File>
<File>
<FileName>e34.c</FileName>
<FileType>1</FileType>
<FilePath>..\..\src\Driver\e34.c</FilePath>
</File>
<File>
<FileName>led.c</FileName>
<FileType>1</FileType>
<FilePath>..\..\src\Driver\led.c</FilePath>
</File>
<File>
<FileName>ring_buffer.c</FileName>
<FileType>1</FileType>
<FilePath>..\..\src\Driver\ring_buffer.c</FilePath>
</File>
<File>
<FileName>stepper_s.c</FileName>
<FileType>1</FileType>
<FilePath>..\..\src\Driver\stepper_s.c</FilePath>
</File>
<File>
<FileName>usart.c</FileName>
<FileType>1</FileType>
<FilePath>..\..\src\Driver\usart.c</FilePath>
</File>
</Files>
</Group>
<Group>
@ -518,6 +538,11 @@
<FileType>1</FileType>
<FilePath>..\..\src\User\main.c</FilePath>
</File>
<File>
<FileName>shell.c</FileName>
<FileType>1</FileType>
<FilePath>..\..\src\User\shell.c</FilePath>
</File>
</Files>
</Group>
</Groups>

10
doc/测试数据.md Normal file
View File

@ -0,0 +1,10 @@
# 步进电机测试文档
## 电机规格
42步进电机,步距角1.8°
## 电机驱动频率:
arr 150~5000
定时器配置T(arr) =1/(84M/(prescale-1)) = 1us
频率 = 1000000/arr = 200~6.666kHz

View File

@ -12,6 +12,8 @@
/* Includes ------------------------------------------------------------------*/
#include "bsp.h"
/* Definition ----------------------------------------------------------------*/
#define STEP_TIM TIM2
#define STEP_EN GPIO_SetBits(GPIOB, GPIO_Pin_4)
#define ANGEL_PER_STEP (1.8f) //步距角1.8°
@ -22,40 +24,54 @@
#define ACC_TIME_DIV 100 //加速阶段离散的点的个数
#define TIME_PER_DIV ACC_TIME / ACC_TIME_DIV //每个离散点持续的时间
#define STEP_MIN_FREQ 2000 //电机最小驱动频率
#define STEP_MAX_FREQ 10000 //电机最大驱动频率
typedef enum {
acc,
avg,
dec
} STEP_STATE; //电机运行状态机
typedef struct
{
u8 id; //电机id
u8 dir_pos; //电机正向方向(顺时针)
u16 div; //电机驱动细分
u8 running; //标记电机是否在运行
char en;
char dir;
long step;
int speed;
int target_speed;
u8 running; //标记电机是否在运行
u8 en;
u8 dir;
u32 step;
// u32 speed;
// u32 target_speed;
u32 current_puase; //当前的脉冲数
u32 current_step; //当前的步数
u32 passed_puase; //走过的脉冲数
u32 passed_step; //走过的步数
u32 target_puase; //设置的目标步数
u32 f_puase; //当前频率脉冲数本算法S型加减速的过程是离散的每个点频率运行一定步数
//u32 f_puase; //当前频率脉冲数本算法S型加减速的过程是离散的每个点频率运行一定步数
u16 f_index; //当前频率在表中所处位置()
u16 *table_step; //步数表
u16 *table_time; //时间表即当前频率电机一个脉冲所需时间用于赋值定时器arr
u32 avg_step; //匀速阶段步数
u32 step_time; //在某一频率离散点持续的时间
u32 start_step; //启动阶段步数
u16 start_f; //启动阶段的频率个数
u32 stop_step; //停止阶段步数
TIM_TypeDef *TIMx; //电机的驱动定时器
STEP_STATE state;
} STEPPER;
/* Exported ------------------------------------------------------------------*/
extern STEPPER *pmotor;
extern STEPPER stepper;
void stepper_tim_init(u16 arr, u16 psc);
void stepper_pwm_init(u16 arr, u16 psc);
void stepper_set_enable(u8 value);
void stepper_set_dir(u8 value);
void Motor_PWM_Init(void);
#endif
/*********************************END OF FILE**********************************/

View File

@ -30,7 +30,7 @@ void stepper_gpio_init(void)
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; //上拉
GPIO_Init(GPIOB, &GPIO_InitStructure); //初始化
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_3; //PB3-PUL
// GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; //复用功能(如果是采用定时器PWM输出需要配制成AF模式)
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; //复用功能(如果是采用定时器PWM输出需要配制成AF模式)
GPIO_Init(GPIOB, &GPIO_InitStructure); //初始化PB3
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10; //PA10-DIR
GPIO_Init(GPIOA, &GPIO_InitStructure);
@ -80,7 +80,6 @@ void stepper_pwm_init(u16 arr, u16 psc)
TIM_OCInitTypeDef TIM_OCInitStructure;
stepper_gpio_init();
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE); //TIM2时钟使能
GPIO_PinAFConfig(GPIOB, GPIO_PinSource3, GPIO_AF_TIM2); //GPIOB3复用为定时器2
TIM_TimeBaseStructure.TIM_Prescaler = psc - 1; //定时器分频
@ -91,17 +90,16 @@ void stepper_pwm_init(u16 arr, u16 psc)
//初始化TIM2 Channel2 PWM模式
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; //选择定时器模式:TIM脉冲宽度调制模式2
// TIM_OCInitStructure.TIM_Pulse = arr >> 1;
TIM_OCInitStructure.TIM_Pulse = (arr - 1) >> 1;
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; //比较输出使能
// TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Disable; //??????
// TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Disable; //高级定时器互补输出
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High; //输出极性:TIM输出比较极性低
TIM_OC2Init(TIM2, &TIM_OCInitStructure); //根据T指定的参数初始化外设TIM1 4OC1
TIM_OC2Init(TIM2, &TIM_OCInitStructure); //根据T指定的参数初始化外设
TIM_OC2PreloadConfig(TIM2, TIM_OCPreload_Enable); //使能TIM2在CCR2上的预装载寄存器
TIM_ARRPreloadConfig(TIM2, ENABLE); //ARPE使能
// TIM_CCPreloadControl(TIM2, ENABLE);
// TIM_CtrlPWMOutputs(TIM4, ENABLE);
// TIM_Cmd(TIM2, DISABLE); //使能TIM2
TIM_CCPreloadControl(TIM2, ENABLE);
TIM_CtrlPWMOutputs(TIM2, ENABLE);
TIM_Cmd(TIM2, ENABLE); //使能TIM2
}

View File

@ -9,9 +9,16 @@
*******************************************************************************/
/* Includes ------------------------------------------------------------------*/
#include "stepper.h"
#include "shell.h"
/* Definition ----------------------------------------------------------------*/
STEPPER motor;
STEPPER motor = {.id = 1, .dir_pos = 1, .div = 4, .TIMx = STEP_TIM};
STEPPER *pmotor = &motor;
u16 table_time[ACC_TIME_DIV] = {4187, 4117, 4042, 3963, 3880, 3791, 3698, 3601, 3500, 3394, 3286, 3173,
3058, 2941, 2822, 2702, 2581, 2461, 2341, 2222, 2104, 1990, 1878, 1769, 1663, 1562, 1465, 1372, 1284, 1200,
1121, 1047, 977, 912, 851, 794, 742, 693, 648, 606, 568, 532, 500, 470, 443, 418, 395, 374, 355, 338, 322, 308,
295, 283, 272, 262, 253, 245, 237, 231, 225, 219, 214, 210, 206, 202, 198, 195, 193, 190, 188, 186, 184, 182, 181,
179, 178, 177, 176, 175, 174, 173, 173, 172, 172, 171, 171, 170, 170, 169, 169, 169, 169, 168, 168, 168, 168, 168, 167, 167};
/* Functions -----------------------------------------------------------------*/
/*******************************************************************************
* @brief GPIO口初始化
@ -21,16 +28,22 @@ STEPPER *pmotor = &motor;
*******************************************************************************/
void Motor_GPIO_Init(void)
{
// GPIO_InitTypeDef GPIO_InitStructure;
// RCC_APB2PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);
// GPIO_InitStructure.GPIO_Pin = GPIO_Pin_1 | GPIO_Pin_2 | GPIO_Pin_3 | GPIO_Pin_4;
// GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
// GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
// GPIO_Init(GPIOA, &GPIO_InitStructure);
// GPIO_ResetBits(GPIOA, GPIO_Pin_1 | GPIO_Pin_2 | GPIO_Pin_3 | GPIO_Pin_4);
GPIO_InitTypeDef GPIO_InitStructure;
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA | RCC_AHB1Periph_GPIOB, ENABLE); //使能PORTA PORTB时钟
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4; //PB4-EN
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; //普通输出模式
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; //推挽输出
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz; //100MHz
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; //上拉
GPIO_Init(GPIOB, &GPIO_InitStructure); //初始化
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_3; //PB3-PUL
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; //复用功能(如果是采用定时器PWM输出需要配制成AF模式)
GPIO_Init(GPIOB, &GPIO_InitStructure); //初始化PB3
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10; //PA10-DIR
GPIO_Init(GPIOA, &GPIO_InitStructure);
}
#if ADV_TIM_ENABLE
#if 0 //高级定时器
/*******************************************************************************
* @brief S型电机驱动
* @param None
@ -53,8 +66,8 @@ void Initial_PWM_Motor1(void)
NVIC_InitStructure.NVIC_IRQChannelSubPriority = PWM1_SubPriority;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
TIM_BaseInitStructure.TIM_Period = 1000;
TIM_BaseInitStructure.TIM_Prescaler = 5;
TIM_BaseInitStructure.TIM_Period = 1000-1;
TIM_BaseInitStructure.TIM_Prescaler = 84-1;
TIM_BaseInitStructure.TIM_ClockDivision = 0;
TIM_BaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_BaseInitStructure.TIM_RepetitionCounter = 0;
@ -63,14 +76,14 @@ void Initial_PWM_Motor1(void)
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM2; //PWM2模式
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; //信号输出到对应的输出引脚
TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable; //互补信号输出到对应的输出引脚
TIM_OCInitStructure.TIM_Pulse = 50; //脉冲宽度
TIM_OCInitStructure.TIM_Pulse = 500; //脉冲宽度
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_Low; //互补输出高电平有效
TIM_OCInitStructure.TIM_OCNPolarity = TIM_OCNPolarity_High; //互补输出高电平有效
TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Reset; //输出空闲状态为1
TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCIdleState_Reset; //互补输出空闲状态为0
TIM_OC1Init(TIM1, &TIM_OCInitStructure); //OC1通道初始化
TIM_OC2Init(TIM1, &TIM_OCInitStructure); //OC1通道初始化
TIM_OC1PreloadConfig(TIM1, TIM_OCPreload_Enable);
TIM_OC2PreloadConfig(TIM1, TIM_OCPreload_Enable);
TIM_ARRPreloadConfig(TIM1, ENABLE);
//清中断,以免一启用中断后立即产生中断
@ -95,6 +108,7 @@ void Motor_PWM_Init(void)
TIM_OCInitTypeDef TIM_OCInitStructure;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);
GPIO_PinAFConfig(GPIOB, GPIO_PinSource3, GPIO_AF_TIM2); //GPIOB3复用为定时器2
Motor_GPIO_Init();
TIM_DeInit(TIM2);
//中断NVIC设置允许中断设置优先级
@ -103,8 +117,8 @@ void Motor_PWM_Init(void)
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1; //响应优先级1
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; //允许中断
NVIC_Init(&NVIC_InitStructure);
TIM_BaseInitStructure.TIM_Period = 1000;
TIM_BaseInitStructure.TIM_Prescaler = 5;
TIM_BaseInitStructure.TIM_Period = 160;
TIM_BaseInitStructure.TIM_Prescaler = 84 - 1;
TIM_BaseInitStructure.TIM_ClockDivision = 0;
TIM_BaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_BaseInitStructure.TIM_RepetitionCounter = 0;
@ -113,21 +127,20 @@ void Motor_PWM_Init(void)
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM2; //PWM2模式
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; //信号输出到对应的输出引脚
TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable; //互补信号输出到对应的输出引脚
TIM_OCInitStructure.TIM_Pulse = 500; //脉冲宽度
TIM_OCInitStructure.TIM_Pulse = 80; //脉冲宽度
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_Low; //互补输出高电平有效
TIM_OCInitStructure.TIM_OCNPolarity = TIM_OCNPolarity_High; //互补输出高电平有效
TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Reset; //输出空闲状态为1
TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCIdleState_Reset; //互补输出空闲状态为0
TIM_OC1Init(TIM2, &TIM_OCInitStructure); //OC1通道初始化
TIM_OC2Init(TIM2, &TIM_OCInitStructure); //OC1通道初始化
TIM_OC1PreloadConfig(TIM2, TIM_OCPreload_Enable);
TIM_ARRPreloadConfig(TIM2, ENABLE);
//清中断,以免一启用中断后立即产生中断
TIM_ClearFlag(TIM2, TIM_FLAG_Update);
//使能TIM1中断源
TIM_ClearFlag(TIM2, TIM_FLAG_Update); //清除溢出中断标志
TIM_ITConfig(TIM2, TIM_IT_Update, ENABLE);
TIM_Cmd(TIM2, DISABLE);
//TIM_CtrlPWMOutputs(TIM2,ENABLE); //使能PWM输出
TIM_OC2PreloadConfig(TIM2, TIM_OCPreload_Enable); //使能TIM2在CCR2上的预装载寄存器
TIM_ARRPreloadConfig(TIM2, ENABLE); //ARPE使能
TIM_CCPreloadControl(TIM2, ENABLE);
TIM_CtrlPWMOutputs(TIM2, ENABLE);
//TIM_Cmd(TIM2, ENABLE); //使能TIM2
}
/*******************************************************************************
@ -152,6 +165,7 @@ void MOTOR_Init(void)
/* 初始化电机参数 */
}
int test;
/*******************************************************************************
* @brief 2
* @param None
@ -160,64 +174,83 @@ void MOTOR_Init(void)
*******************************************************************************/
void TIM2_IRQHandler(void)
{
TIM2->SR = (u16)~TIM_FLAG_Update;
if (1 == pmotor->en)
static int cnt = 0;
if (TIM_GetITStatus(TIM2, TIM_IT_Update) != RESET)
{
TIM_ClearITPendingBit(TIM2, TIM_FLAG_Update);
if (1 == pmotor->en)//若电机使能
{
//位置计算
if (pmotor->dir_pos == pmotor->dir)
// if (pmotor->dir_pos == pmotor->dir)
// {
// pmotor->current_puase++;
// }
// else
// {
// pmotor->current_puase--;
// }
// pmotor->current_step = pmotor->current_puase / pmotor->div;
pmotor->step_time += table_time[pmotor->f_index];
if (cnt++ > pmotor->div) // 步进电机每走一步
{
pmotor->current_puase++;
cnt = 0;
pmotor->passed_puase++; //总脉冲个数
switch (pmotor->state)
{
case acc: //加速阶段
test = pmotor->step >> 1;
if (pmotor->passed_puase < test)
{
if (pmotor->step_time > TIME_PER_DIV * 1000) //一个频率的步数走完
{
pmotor->f_index++;
//判断加速阶段是否结束
if (pmotor->f_index > ACC_TIME_DIV - 2)
{
//规划平速运行阶段和减速运行阶段
pmotor->avg_step = pmotor->step - pmotor->passed_puase * 2;
pmotor->state = avg;
}
else
{
pmotor->current_puase--;
}
pmotor->current_step = pmotor->current_puase / pmotor->div;
pmotor->passed_puase++; //总脉冲个数
pmotor->f_puase++; //以该频率脉冲输出的脉冲个数
//对称反转???
// if (pmotor->RevetDot == pmotor->PulsesHaven)
// {
// pmotor->pulsecount = pmotor->Step_Table[pmotor->CurrentIndex];
// }
if (pmotor->f_puase >= pmotor->table_step[pmotor->f_index]) //一个频率的步数走完
{
if (pmotor->passed_puase <= pmotor->start_step) //如果是起步阶段
{
if (pmotor->f_index < pmotor->start_f - 1)
{
pmotor->f_index++; //进入下一个频率阶段
pmotor->f_puase = 0;
if (pmotor->f_index >= pmotor->start_f) //启动阶段的步数没有走完,禁止进入下一阶段的频率
pmotor->f_index = pmotor->start_f;
TIM2->ARR = table_time[pmotor->f_index];
test = table_time[pmotor->f_index] >> 1;
TIM_SetCompare2(TIM2, test);
pmotor->step_time = 0;
}
}
if (pmotor->target_puase - pmotor->passed_puase <= pmotor->stop_step) //停止阶段
{
// if (pmotor->f_index < pmotor->StartTableLength - 1)
// {
// pmotor->f_index = pmotor->StartTableLength + pmotor->StopTableLength - pmotor->CurrentIndex;
// }
// pmotor->f_index++;
// pmotor->f_puase = 0;
// if (pmotor->f_index >= pmotor->StartTableLength + pmotor->StopTableLength)
// pmotor->CurrentIndex = pmotor->StartTableLength + pmotor->StopTableLength - 1;
}
pmotor->TIMx->ARR = pmotor->table_time[pmotor->f_index]; //设置周期
pmotor->TIMx->CCR1 = (pmotor->table_time[pmotor->f_index]) >> 1; //设置占空比
}
//旋转预定脉冲数停止running=0可以进行下一次旋转
if (pmotor->passed_puase >= pmotor->target_puase && pmotor->passed_puase > 3)
else //两段式
{
pmotor->en = 0;
pmotor->running = 0;
pmotor->f_index = 0;
TIM_Cmd(pmotor->TIMx, DISABLE); //DISABLE
pmotor->state = dec; //进入减速阶段
}
break;
case avg: //匀速阶段
pmotor->avg_step--;
if (pmotor->avg_step == 0)
pmotor->state = dec;
break;
case dec: //减速阶段
if (pmotor->passed_puase < pmotor->step)
{
if (pmotor->step_time > TIME_PER_DIV * 1000) //一个频率的步数走完
{
pmotor->f_index--;
TIM2->ARR = table_time[pmotor->f_index];
test = table_time[pmotor->f_index] >> 1;
TIM_SetCompare2(TIM2, test);
pmotor->step_time = 0;
if (pmotor->f_index == 0)
pmotor->f_index = 1; //修正,防止减速阶段结束了,步数没走完
}
}
else
{
TIM_Cmd(TIM2, DISABLE); //失能TIM2
}
break;
}
}
}
}
}

View File

@ -78,7 +78,7 @@ struct __FILE
FILE __stdout;
//定义_sys_exit()以避免使用半主机模式
_sys_exit(int x)
void _sys_exit(int x)
{
x = x;
}

View File

@ -23,11 +23,20 @@
*******************************************************************************/
int main(void)
{
int freq = 5000;//150可以直接启动100需要一个启动过程
SysTick_init();
E34_Init();
stepper_tim_init(50, 84);
// stepper_pwm_init(125, 84);
//stepper_tim_init(50, 84);
Motor_PWM_Init();
//stepper_pwm_init(freq, 84);
//TIM_SetCompare2(TIM2,freq>>1);
stepper_set_enable(0);
stepper_set_dir(0);
pmotor->en = 1;
pmotor->step_time = 0;
pmotor->state = acc;
pmotor->step = 5000;
TIM_Cmd(TIM2, ENABLE); //使能TIM2
while (1)
{
shell_loop();

27
tool/S_calc.py Normal file
View File

@ -0,0 +1,27 @@
# encoding utf8
"""
计算步进电机S型加减速算法所需的S型曲线表
"""
import numpy as np
import matplotlib.pyplot as plt
import math
np.set_printoptions(suppress=True)
# 电机运行最低频率
step_min_freq = 200
# 电机运行最高频率
step_max_freq = 6000
x = np.arange(-5, 5, 0.1)
y = 1/(1+np.exp(-x))
time_table = (1000000/(step_min_freq+(step_max_freq-step_min_freq)*y))
# plt.plot(x,y)
# plt.show()
for data in time_table.tolist():
print(int(data), end=',')
# print(data)