547 lines
12 KiB
C
547 lines
12 KiB
C
#include "stepmotor.h"
|
|
#include "math.h"
|
|
#include "gpio_devs.h"
|
|
#include "time.h"
|
|
|
|
|
|
static _stepmotor *root = NULL;
|
|
|
|
#define s_table_count 50
|
|
uint32_t curve_s_table[s_table_count] = {0};
|
|
|
|
|
|
_stepmotor motor1;
|
|
_stepmotor motor2;
|
|
_stepmotor motor3;
|
|
_stepmotor motor4;
|
|
_stepmotor motor5;
|
|
|
|
|
|
|
|
_gpio_group start_1;
|
|
_gpio_group end_1;
|
|
_gpio_group pul_1;
|
|
_gpio_group ena_1;
|
|
_gpio_group dir_1;
|
|
|
|
_gpio_group start_2;
|
|
_gpio_group end_2;
|
|
_gpio_group pul_2;
|
|
_gpio_group ena_2;
|
|
_gpio_group dir_2;
|
|
|
|
_gpio_group start_3;
|
|
_gpio_group end_3;
|
|
_gpio_group pul_3;
|
|
_gpio_group ena_3;
|
|
_gpio_group dir_3;
|
|
|
|
_gpio_group start_4;
|
|
_gpio_group end_4;
|
|
_gpio_group pul_4;
|
|
_gpio_group ena_4;
|
|
_gpio_group dir_4;
|
|
|
|
_gpio_group start_5;
|
|
_gpio_group end_5;
|
|
_gpio_group pul_5;
|
|
_gpio_group ena_5;
|
|
_gpio_group dir_5;
|
|
|
|
|
|
//buff_pa是速度表数组。count_va是加速次数。pan_right_va是右移量，代表了精度，即右移多少后，y强制为0，一般用7，最后一次速度在99.9%。
|
|
//count_va越大，S曲线越平缓，加速过程越平滑，加速时间越长；count_va越小，S曲线越陡峭，加速过程越急剧，加速时间越短。
|
|
void CurveS_init(uint32_t *buff, uint16_t count, uint8_t pan_right)
|
|
{
|
|
|
|
for (uint16_t x = 0; x < count; x++)
|
|
{
|
|
buff[x] = 3.0 / (10.0 + exp(((float)pan_right * 2 / count * -x) + pan_right)) * 4000;
|
|
//buff_pa[x] = 1.0 / (1.0 + pow(sqrt(m_e_v), (float)pan_right_va * 2 / count_va * -x + pan_right_va));
|
|
//这是通用计算方法，pow函数的第一个参数越接近1，该算式计算出的曲线越平缓，可以改的和线性加减速无异
|
|
}
|
|
}
|
|
|
|
|
|
void stepmotor_init(void)
|
|
{
|
|
CurveS_init(curve_s_table, s_table_count, 7);
|
|
|
|
start_1.port = BSP_DI_1_GPIO_PORT;
|
|
start_1.pin = BSP_DI_1_PIN;
|
|
end_1.port = BSP_DI_2_GPIO_PORT;
|
|
end_1.pin = BSP_DI_2_PIN;
|
|
|
|
pul_1.port = BSP_PUL_1_GPIO_PORT;
|
|
pul_1.pin = BSP_PUL_1_PIN;
|
|
ena_1.port = BSP_ENA_1_GPIO_PORT;
|
|
ena_1.pin = BSP_ENA_1_PIN;
|
|
dir_1.port = BSP_DIR_1_GPIO_PORT;
|
|
dir_1.pin = BSP_DIR_1_PIN;
|
|
|
|
stepmotor_install(&motor1,"motor1",start_1,end_1,pul_1,ena_1,dir_1);
|
|
|
|
|
|
|
|
start_2.port = BSP_DI_3_GPIO_PORT;
|
|
start_2.pin = BSP_DI_3_PIN;
|
|
end_2.port = BSP_DI_4_GPIO_PORT;
|
|
end_2.pin = BSP_DI_4_PIN;
|
|
|
|
pul_2.port = BSP_PUL_2_GPIO_PORT;
|
|
pul_2.pin = BSP_PUL_2_PIN;
|
|
ena_2.port = BSP_ENA_2_GPIO_PORT;
|
|
ena_2.pin = BSP_ENA_2_PIN;
|
|
dir_2.port = BSP_DIR_2_GPIO_PORT;
|
|
dir_2.pin = BSP_DIR_2_PIN;
|
|
|
|
|
|
stepmotor_install(&motor2,"motor2",start_2,end_2,pul_2,ena_2,dir_2);
|
|
|
|
|
|
start_3.port = BSP_DI_5_GPIO_PORT;
|
|
start_3.pin = BSP_DI_5_PIN;
|
|
end_3.port = BSP_DI_6_GPIO_PORT;
|
|
end_3.pin = BSP_DI_6_PIN;
|
|
|
|
pul_3.port = BSP_PUL_3_GPIO_PORT;
|
|
pul_3.pin = BSP_PUL_3_PIN;
|
|
ena_3.port = BSP_ENA_3_GPIO_PORT;
|
|
ena_3.pin = BSP_ENA_3_PIN;
|
|
dir_3.port = BSP_DIR_3_GPIO_PORT;
|
|
dir_3.pin = BSP_DIR_3_PIN;
|
|
|
|
stepmotor_install(&motor3,"motor3",start_3,end_3,pul_3,ena_3,dir_3);
|
|
|
|
|
|
start_4.port = BSP_DI_7_GPIO_PORT;
|
|
start_4.pin = BSP_DI_7_PIN;
|
|
end_4.port = BSP_DI_8_GPIO_PORT;
|
|
end_4.pin = BSP_DI_8_PIN;
|
|
|
|
pul_4.port = BSP_PUL_4_GPIO_PORT;
|
|
pul_4.pin = BSP_PUL_4_PIN;
|
|
ena_4.port = BSP_ENA_4_GPIO_PORT;
|
|
ena_4.pin = BSP_ENA_4_PIN;
|
|
dir_4.port = BSP_DIR_4_GPIO_PORT;
|
|
dir_4.pin = BSP_DIR_4_PIN;
|
|
|
|
stepmotor_install(&motor4,"motor4",start_4,end_4,pul_4,ena_4,dir_4);
|
|
|
|
|
|
|
|
start_5.port = BSP_DI_9_GPIO_PORT;
|
|
start_5.pin = BSP_DI_9_PIN;
|
|
end_5.port = BSP_DI_10_GPIO_PORT;
|
|
end_5.pin = BSP_DI_10_PIN;
|
|
|
|
pul_5.port = BSP_PUL_5_GPIO_PORT;
|
|
pul_5.pin = BSP_PUL_5_PIN;
|
|
ena_5.port = BSP_ENA_5_GPIO_PORT;
|
|
ena_5.pin = BSP_ENA_5_PIN;
|
|
dir_5.port = BSP_DIR_5_GPIO_PORT;
|
|
dir_5.pin = BSP_DIR_5_PIN;
|
|
|
|
stepmotor_install(&motor5,"motor5",start_5,end_5,pul_5,ena_5,dir_5);
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
uint8_t stepmotor_install(_stepmotor *e,char *name,_gpio_group start,
|
|
_gpio_group end,
|
|
_gpio_group pul,
|
|
_gpio_group ena,
|
|
_gpio_group dir)
|
|
{
|
|
e->channel = 0;
|
|
e->name = name;
|
|
e->next_ptr = NULL;
|
|
|
|
e->step_index = s_table_count - 1;//获取最大的延迟，因为最开始电机肯定是停止的
|
|
|
|
|
|
e->step_delay = curve_s_table[e->step_index];
|
|
e->running = 0;
|
|
e->isGetPos = 0;
|
|
e->step_count = 0;
|
|
|
|
e->stateCount = 0;
|
|
e->step_len = 50000;
|
|
|
|
//had been initialized as input
|
|
e->start.port = start.port;
|
|
e->start.pin = start.pin;
|
|
e->end.port = end.port;
|
|
e->end.pin = end.pin;
|
|
//===========
|
|
e->pul.port = pul.port;
|
|
e->pul.pin = pul.pin;
|
|
e->ena.port = ena.port;
|
|
e->ena.pin = ena.pin;
|
|
e->dir.port = dir.port;
|
|
e->dir.pin = dir.pin;
|
|
|
|
GPIO_enable_rcu(e->pul.port);
|
|
gpio_init(e->pul.port, GPIO_MODE_OUT_PP, GPIO_OSPEED_50MHZ, e->pul.pin);
|
|
|
|
GPIO_enable_rcu(e->ena.port);
|
|
gpio_init(e->ena.port, GPIO_MODE_OUT_PP, GPIO_OSPEED_50MHZ, e->ena.pin);
|
|
|
|
GPIO_enable_rcu(e->dir.port);
|
|
gpio_init(e->dir.port, GPIO_MODE_OUT_PP, GPIO_OSPEED_50MHZ, e->dir.pin);
|
|
|
|
|
|
|
|
if (root)
|
|
{
|
|
_stepmotor *last = root;
|
|
while (last->next_ptr)
|
|
{
|
|
last = last->next_ptr;
|
|
}
|
|
last->next_ptr = e;
|
|
e->channel = last->channel + 1;
|
|
}
|
|
else
|
|
{
|
|
root = e;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
bool stepmotor_getStart(_stepmotor *e)
|
|
{
|
|
return gpio_input_bit_get(e->start.port, e->start.pin) ? true : false;
|
|
}
|
|
|
|
bool stepmotor_getEnd(_stepmotor *e)
|
|
{
|
|
return gpio_input_bit_get(e->end.port, e->end.pin) ? true : false;
|
|
}
|
|
|
|
void stepmotor_setPul(_stepmotor *e,bool state)
|
|
{
|
|
if (state)
|
|
gpio_bit_set(e->pul.port, e->pul.pin);
|
|
else
|
|
gpio_bit_reset(e->pul.port, e->pul.pin);
|
|
}
|
|
|
|
void stepmotor_setDir(_stepmotor *e,bool state)
|
|
{
|
|
if (state)
|
|
gpio_bit_set(e->dir.port, e->dir.pin);
|
|
else
|
|
gpio_bit_reset(e->dir.port, e->dir.pin);
|
|
}
|
|
|
|
void stepmotor_setEna(_stepmotor *e,bool state)
|
|
{
|
|
if (state)
|
|
gpio_bit_set(e->ena.port, e->ena.pin);
|
|
else
|
|
gpio_bit_reset(e->ena.port, e->ena.pin);
|
|
}
|
|
|
|
|
|
void stepmotor_getStepLen(_stepmotor *e)
|
|
{
|
|
stepmotor_setPul(e,false);//拉低电平
|
|
stepmotor_setDir(e,true);//正向运动
|
|
stepmotor_setEna(e,true);//运动
|
|
|
|
e->step_delay = 20;
|
|
e->running = 0;
|
|
e->isGetPos = 1;
|
|
}
|
|
|
|
|
|
|
|
void stepmotor_setMove(_stepmotor *e,int8_t state)//-1 0 1
|
|
{
|
|
if(state != 0)
|
|
{
|
|
stepmotor_setPul(e,false);//拉低电平
|
|
|
|
stepmotor_setEna(e,true);//运动
|
|
|
|
e->step_delay = 20;
|
|
e->step_current = 0;
|
|
e->isGetPos = 0;
|
|
e->step_len = 50000;
|
|
e->state = motor_accelerate;
|
|
|
|
if(state > 0)
|
|
{
|
|
e->running = 1;
|
|
stepmotor_setDir(e,false);//正向运动
|
|
}
|
|
else
|
|
{
|
|
e->running = -1;
|
|
stepmotor_setDir(e,true);//反向运动
|
|
}
|
|
|
|
}
|
|
else//0，停止
|
|
{
|
|
stepmotor_setPul(e,false);//拉低电平
|
|
stepmotor_setDir(e,false);//反向运动
|
|
stepmotor_setEna(e,false);//停止
|
|
e->step_delay = 20;
|
|
e->running = 0;
|
|
e->isGetPos = 0;
|
|
}
|
|
}
|
|
|
|
void stepmotor_thread(void)//20kHz
|
|
{
|
|
if (root)
|
|
{
|
|
_stepmotor *e = root;
|
|
while (e)
|
|
{
|
|
if(e->running == 0)//停止运行，进入测试模式
|
|
{
|
|
if(e->isGetPos == 0)
|
|
{
|
|
stepmotor_setEna(e,false);//停止
|
|
stepmotor_setPul(e,false);
|
|
stepmotor_setDir(e,false);
|
|
}
|
|
else if(e->isGetPos > 0)//正方向 获取结束
|
|
{
|
|
e->step_count ++;
|
|
|
|
if(e->step_delay <= e->step_count)//超时之后开始下一个周期
|
|
{
|
|
e->step_count = 0;
|
|
}
|
|
else if((e->step_delay * 0.5) <= e->step_count)
|
|
{
|
|
stepmotor_setPul(e,false);//拉低电平
|
|
}
|
|
else if(1 <= e->step_count)
|
|
{
|
|
stepmotor_setPul(e,true);//抬高电平
|
|
}
|
|
|
|
if(stepmotor_getEnd(e) == true)//开始按键还没检测到，所以可以一直运行
|
|
{
|
|
e->stateCount ++;
|
|
|
|
if(e->stateCount >= 3)//检测3个脉冲后如果还是高电平，说明没有误触发
|
|
{
|
|
e->stateCount = 0;
|
|
e->isGetPos = -1;
|
|
e->step_count = 0;
|
|
e->step_len = 0;
|
|
stepmotor_setEna(e,false);//停止
|
|
stepmotor_setDir(e,false);
|
|
stepmotor_setEna(e,true);//停止
|
|
}
|
|
}
|
|
else
|
|
{
|
|
e->stateCount = 0;
|
|
}
|
|
|
|
}
|
|
else if(e->isGetPos < 0)//反方向 获取开始
|
|
{
|
|
e->step_count ++;
|
|
|
|
if(e->step_delay <= e->step_count)//超时之后开始下一个周期
|
|
{
|
|
e->step_count = 0;
|
|
}
|
|
else if((e->step_delay * 0.5) <= e->step_count)
|
|
{
|
|
stepmotor_setPul(e,false);//拉低电平
|
|
e->step_len ++;
|
|
}
|
|
else if(1 <= e->step_count)
|
|
{
|
|
stepmotor_setPul(e,true);//抬高电平
|
|
}
|
|
|
|
if(stepmotor_getStart(e) == true)//开始按键还没检测到，所以可以一直运行
|
|
{
|
|
e->stateCount ++;
|
|
|
|
if(e->stateCount >= 3)//检测3个脉冲后如果还是高电平，说明没有误触发
|
|
{
|
|
e->stateCount = 0;
|
|
e->isGetPos = 0;
|
|
e->step_count = 0;
|
|
}
|
|
|
|
}
|
|
else
|
|
{
|
|
e->stateCount = 0;
|
|
}
|
|
}
|
|
|
|
}
|
|
else
|
|
{
|
|
//计算时间是否合适
|
|
e->step_count++;//每个周期加一个
|
|
if(e->step_delay <= e->step_count)//超时之后开始下一个周期
|
|
{
|
|
//计时归零
|
|
e->step_count = 0;
|
|
|
|
e->step_current++;
|
|
|
|
|
|
//延迟长度读取，并且状态切换计算
|
|
if(e->state == motor_accelerate)//加速
|
|
{
|
|
e->step_index --;
|
|
if(e->step_index <= 0)
|
|
{
|
|
e->step_index = 0;
|
|
e->state = motor_speed_max;//状态切换到最大速度
|
|
}
|
|
e->step_delay = curve_s_table[e->step_index];
|
|
}
|
|
else if(e->state == motor_speed_max)//最大速度
|
|
{
|
|
//如果计数大于一个加速和一个正常运行长度时，就开始减速
|
|
if(e->step_current > (e->step_len - s_table_count))
|
|
{
|
|
e->state = motor_decelerate;//状态切换到减速
|
|
}
|
|
|
|
e->step_delay = curve_s_table[0];
|
|
}
|
|
else if(e->state == motor_decelerate)//减速
|
|
{
|
|
e->step_index ++;
|
|
if(e->step_index >= s_table_count)
|
|
{
|
|
e->step_index = s_table_count - 1;
|
|
e->state = motor_stop;
|
|
}
|
|
e->step_delay = curve_s_table[e->step_index];
|
|
}
|
|
else//停止
|
|
{
|
|
//还在一直低速运行，直到碰到开关才结束
|
|
e->step_index = s_table_count - 1;
|
|
e->step_delay = curve_s_table[e->step_index];
|
|
e->running = 0;
|
|
}
|
|
|
|
|
|
if(e->running < 0)//往后运行
|
|
{
|
|
stepmotor_setDir(e,true);
|
|
|
|
if(stepmotor_getStart(e) != true)//开始按键还没检测到，所以可以一直运行
|
|
{
|
|
//如果不处理，那么没有传感器的时候可能停不下来
|
|
e->stateCount = 0;
|
|
}
|
|
else//已经检测到
|
|
{
|
|
e->stateCount ++;
|
|
|
|
if(e->stateCount >= 3)//检测3个脉冲后如果还是高电平，说明没有误触发
|
|
{
|
|
e->stateCount = 0;
|
|
e->isGetPos = 0;
|
|
e->step_count = 0;
|
|
e->step_current = 0;
|
|
//e->step_len = 0;
|
|
e->running = 0;//设置为0，下一个周期时可以停止运行
|
|
}
|
|
}
|
|
}
|
|
else if(e->running > 0)//往前运行
|
|
{
|
|
stepmotor_setDir(e,false);
|
|
|
|
if(stepmotor_getEnd(e) != true)//开始按键还没检测到，所以可以一直运行
|
|
{
|
|
//如果不处理，那么没有传感器的时候可能停不下来
|
|
e->stateCount = 0;
|
|
}
|
|
else
|
|
{
|
|
e->stateCount ++;
|
|
|
|
if(e->stateCount >= 3)//检测3个脉冲后如果还是高电平，说明没有误触发
|
|
{
|
|
e->stateCount = 0;
|
|
e->isGetPos = 0;
|
|
e->step_count = 0;
|
|
e->step_current = 0;
|
|
//e->step_len = 0;
|
|
e->running = 0;//设置为0，下一个周期时可以停止运行
|
|
}
|
|
}
|
|
}
|
|
}
|
|
else if((e->step_delay * 0.5) <= e->step_count)
|
|
{
|
|
stepmotor_setPul(e,false);//拉低电平
|
|
}
|
|
else if(1 <= e->step_count)
|
|
{
|
|
stepmotor_setPul(e,true);//抬高电平
|
|
}
|
|
|
|
|
|
}
|
|
e = e->next_ptr;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
|
|
|
|
void stepmotor_getlen(uint16_t channel)
|
|
{
|
|
switch(channel)
|
|
{
|
|
case 0: stepmotor_getStepLen(&motor1);break;
|
|
case 1: stepmotor_getStepLen(&motor2);break;
|
|
case 2: stepmotor_getStepLen(&motor3);break;
|
|
case 3: stepmotor_getStepLen(&motor4);break;
|
|
case 4: stepmotor_getStepLen(&motor5);break;
|
|
}
|
|
}
|
|
|
|
|
|
void stepmotor_act(uint16_t channel,int8_t state)
|
|
{
|
|
switch(channel)
|
|
{
|
|
case 0: stepmotor_setMove(&motor1,state);break;//-1 0 1
|
|
case 1: stepmotor_setMove(&motor2,state);break;//-1 0 1
|
|
case 2: stepmotor_setMove(&motor3,state);break;//-1 0 1
|
|
case 3: stepmotor_setMove(&motor4,state);break;//-1 0 1
|
|
case 4: stepmotor_setMove(&motor5,state);break;//-1 0 1
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|