430 lines
8.1 KiB
C
430 lines
8.1 KiB
C
#include "basic.h"
|
||
|
||
#define STEP_LIN 80 //直行程步长单位数值（脉冲数）
|
||
//#define DATA_LEN 48 //角行程步长单位数值（脉冲数）
|
||
|
||
|
||
|
||
int mov_flag = 0;
|
||
int zero_flag = 2;
|
||
u32 beep_time = 0;
|
||
|
||
|
||
//步骤
|
||
unsigned char Runmotor_step = 0;
|
||
|
||
//次数
|
||
unsigned int Runmotor_Nums = 0;
|
||
|
||
//电机启动
|
||
//IO口变换为输出模式，输出脉冲信号
|
||
void motor_start(void)
|
||
{
|
||
GPIO_InitTypeDef GPIO_InitStructure;
|
||
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);
|
||
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12;
|
||
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; //输出模式
|
||
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; //推挽输出
|
||
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
|
||
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
|
||
GPIO_Init(GPIOD, &GPIO_InitStructure);
|
||
|
||
//GPIO_ResetBits(GPIOD,GPIO_Pin_12);
|
||
}
|
||
|
||
//电机停止
|
||
//IO口变换为输入模式，脉冲信号无法输出
|
||
void motor_stop(void)
|
||
{
|
||
GPIO_InitTypeDef GPIO_InitStructure;
|
||
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);
|
||
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12;
|
||
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;
|
||
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL ;
|
||
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
|
||
GPIO_Init(GPIOD, &GPIO_InitStructure);
|
||
}
|
||
|
||
//正转
|
||
void FWD(void)
|
||
{
|
||
GPIO_ResetBits(GPIOD,GPIO_Pin_13);//引脚电平置低，电机正转
|
||
}
|
||
|
||
//反转
|
||
void REV(void)
|
||
{
|
||
GPIO_SetBits(GPIOD,GPIO_Pin_13);//引脚电平置高，电机反转
|
||
}
|
||
|
||
/*****************************************************
|
||
*函数名称: motor_mov_slow
|
||
*函数功能: 脉冲输出慢
|
||
*参数说明：
|
||
*****************************************************/
|
||
void motor_mov_slow(void)
|
||
{
|
||
if(it_1ms_flag) //检查1ms定时标志
|
||
{
|
||
it_1ms_flag = 0;//定时标志清零
|
||
GPIO_ToggleBits(GPIOD,GPIO_Pin_12);//PUL电平翻转
|
||
Runmotor_Nums++;
|
||
}
|
||
}
|
||
|
||
/*****************************************************
|
||
*函数名称: motor_mov_fast
|
||
*函数功能: 脉冲输出快
|
||
*参数说明：
|
||
*****************************************************/
|
||
void motor_mov_fast(void)
|
||
{
|
||
if(it_500us_flag) //检查1ms定时标志
|
||
{
|
||
it_500us_flag = 0;//定时标志清零
|
||
GPIO_ToggleBits(GPIOD,GPIO_Pin_12);//PUL电平翻转
|
||
Runmotor_Nums++;
|
||
}
|
||
}
|
||
|
||
//寻找零点
|
||
void set_zero()
|
||
{
|
||
float start_point = 0;
|
||
if(zero_flag == 0)
|
||
{
|
||
FWD();
|
||
motor_start();
|
||
motor_mov_slow();
|
||
if(GPIO_ReadInputDataBit(GPIOE,GPIO_Pin_11) == 0 )//act_length >= 21.3f|| zero_deal()
|
||
{
|
||
Motor_Run = 0;
|
||
if(it_1ms_flag) //检查1ms定时标志
|
||
{
|
||
it_1ms_flag = 0;//定时标志清零
|
||
Runmotor_Nums++;
|
||
}
|
||
if(Runmotor_Nums >= 500)//时间
|
||
{
|
||
Runmotor_Nums = 0;
|
||
zero_length = length;
|
||
zero_flag = 1;
|
||
|
||
}
|
||
}
|
||
}
|
||
else if(zero_flag == 1)
|
||
{
|
||
if(temp > 30)
|
||
start_point = (Rate_dis+1)*2 + 0.15;//测距传感器、装夹治具、标靶治具等三者70℃条件下热膨胀导致的整体偏差系数
|
||
else
|
||
start_point = (Rate_dis+1)*2;
|
||
if(act_length >= start_point)
|
||
{
|
||
//蜂鸣器提醒，置零结束
|
||
if(beep_time < 10000)
|
||
{
|
||
beep_time++;
|
||
GPIO_SetBits(GPIOG,GPIO_Pin_0);
|
||
}
|
||
else
|
||
{
|
||
GPIO_ResetBits(GPIOG,GPIO_Pin_0);
|
||
Motor_Run = 0;
|
||
zero_flag = 2;
|
||
send_set_resp(0xF004, OBJ_DEVICE_ADDR, 50, send_data);//数据发送
|
||
}
|
||
}
|
||
else
|
||
{
|
||
REV();
|
||
motor_start();
|
||
motor_mov_slow();
|
||
}
|
||
}
|
||
else
|
||
return;
|
||
}
|
||
|
||
|
||
//移动到近点
|
||
void go_star()
|
||
{
|
||
REV();
|
||
if(act_length<3.9f)
|
||
REV();
|
||
else if(act_length>4.1f)
|
||
FWD();
|
||
motor_start();
|
||
motor_mov_slow();
|
||
if(act_length<((Rate_dis+1)*2 + 0.02f) && act_length >= (Rate_dis+1)*2)//act_length >= 21.3f
|
||
{
|
||
motor_stop();
|
||
if(beep_time<0xffff)
|
||
beep_time++;
|
||
if(beep_time<10000)
|
||
GPIO_SetBits(GPIOG,GPIO_Pin_0);
|
||
else
|
||
{
|
||
GPIO_ResetBits(GPIOG,GPIO_Pin_0);
|
||
Motor_Run = 0;
|
||
}
|
||
}
|
||
}
|
||
|
||
//标靶贴近传感器
|
||
void go_begin()
|
||
{
|
||
FWD();
|
||
motor_start();
|
||
motor_mov_fast();
|
||
if(act_length<=0.01f)//act_length >= 21.3fGPIO_ReadInputDataBit(GPIOE,GPIO_Pin_11) == 0
|
||
{
|
||
motor_stop();
|
||
if(beep_time<0xffff)
|
||
beep_time++;
|
||
if(beep_time<10000)
|
||
GPIO_SetBits(GPIOG,GPIO_Pin_0);
|
||
else
|
||
{
|
||
GPIO_ResetBits(GPIOG,GPIO_Pin_0);
|
||
Motor_Run = 0;
|
||
}
|
||
}
|
||
}
|
||
|
||
//标靶远离传感器
|
||
void go_end()
|
||
{
|
||
REV();
|
||
motor_start();
|
||
motor_mov_fast();
|
||
if(GPIO_ReadInputDataBit(GPIOE,GPIO_Pin_12) == 0)//act_length >= 21.3f
|
||
{
|
||
motor_stop();
|
||
if(beep_time<0xffff)
|
||
beep_time++;
|
||
if(beep_time<10000)
|
||
GPIO_SetBits(GPIOG,GPIO_Pin_0);
|
||
else
|
||
{
|
||
GPIO_ResetBits(GPIOG,GPIO_Pin_0);
|
||
Motor_Run = 0;
|
||
}
|
||
}
|
||
}
|
||
|
||
float l1 = 0;
|
||
float l2 = 0;
|
||
|
||
//电机点动运行
|
||
void mov_step(void)
|
||
{
|
||
if(Run_Step == 0) return;//步长不能为0
|
||
|
||
switch(Runmotor_step)
|
||
{
|
||
case 1 : //电机运行准备
|
||
{
|
||
|
||
Runmotor_Nums = 0; //运行次数
|
||
mov_flag = 0;
|
||
l1 = length;
|
||
Runmotor_step++;
|
||
}
|
||
break;
|
||
case 2 : //电机运行过程
|
||
{
|
||
motor_start(); //运行
|
||
motor_mov_slow();//运行1个脉冲
|
||
if(Runmotor_Nums >= (Run_Step * STEP_LIN))//运行一个步长，脉冲数由驱动器的细分数决定
|
||
{
|
||
Runmotor_Nums = 0;
|
||
motor_stop(); //马达停止
|
||
Runmotor_step++;
|
||
}
|
||
}
|
||
break;
|
||
case 3 :
|
||
{
|
||
if(it_1ms_flag) //检查1ms定时标志
|
||
{
|
||
it_1ms_flag = 0;//定时标志清零
|
||
Runmotor_Nums++;
|
||
}
|
||
if(Runmotor_Nums >= Run_Inter)//时间300
|
||
{
|
||
Runmotor_Nums = 0;
|
||
l2 = length;
|
||
if(fabsf(l2-l1)*1000 < ((Run_Step*10)-1))
|
||
Runmotor_step = 5;
|
||
else
|
||
{
|
||
Runmotor_step++;
|
||
data_deal(send_data,V_Channel,act_length,temp); //ADC数据、温度、位移数据整理打包
|
||
send_set_resp(0xF001, OBJ_DEVICE_ADDR, 50, send_data);//数据发送
|
||
}
|
||
}
|
||
}
|
||
break;
|
||
case 4 :
|
||
{
|
||
Runmotor_step = 0;
|
||
Motor_Run = 0; //运行标记清除
|
||
}
|
||
break;
|
||
case 5 :
|
||
{
|
||
motor_start(); //运行
|
||
motor_mov_slow();//运行1个脉冲
|
||
if(Runmotor_Nums >= ((Run_Step-(fabsf(l2-l1)*100))*STEP_LIN))//运行缺失的步长，脉冲数由驱动器的细分数决定
|
||
{
|
||
Runmotor_Nums = 0;
|
||
motor_stop(); //马达停止
|
||
Runmotor_step = 6;
|
||
}
|
||
}
|
||
break;
|
||
case 6 :
|
||
{
|
||
if(it_1ms_flag) //检查1ms定时标志
|
||
{
|
||
it_1ms_flag = 0;//定时标志清零
|
||
Runmotor_Nums++;
|
||
}
|
||
if(Runmotor_Nums >= Run_Inter)//时间300
|
||
{
|
||
Runmotor_Nums = 0;
|
||
l2 = length;
|
||
Runmotor_step = 4;
|
||
send_set_resp(0xF001, OBJ_DEVICE_ADDR, 50, send_data);//数据发送
|
||
}
|
||
}
|
||
break;
|
||
default :
|
||
{
|
||
|
||
}
|
||
break;
|
||
}
|
||
}
|
||
|
||
|
||
//处理电机运行
|
||
void Deal_Motor(void)
|
||
{
|
||
//判断电机停止还是运行，运行到起始位还是结束位
|
||
if( Motor_Run == 0)//停止
|
||
{
|
||
motor_stop();
|
||
beep_time = 0;
|
||
GPIO_ResetBits(GPIOG,GPIO_Pin_0);
|
||
}
|
||
else if(Motor_Run == 1)//运行
|
||
{
|
||
if(act_length < 0.001f)
|
||
GPIO_SetBits(GPIOG,GPIO_Pin_0);
|
||
if(Motor_Run >= 1 && Runmotor_step == 0)
|
||
{
|
||
Runmotor_step = 1;
|
||
}
|
||
|
||
//判断电机运行方式
|
||
if(Run_Mode == 0)//点动（方案三）
|
||
{
|
||
//motor_data[2] = 0x00;//发送时反馈的数据
|
||
if(Run_Dir == 0)
|
||
FWD();
|
||
else
|
||
REV();
|
||
mov_step();//点动
|
||
}
|
||
else if(Run_Mode == 1)
|
||
{
|
||
if(Run_Dir == 0)
|
||
FWD();
|
||
else
|
||
REV();
|
||
mov_step();
|
||
//mov_loop();
|
||
}
|
||
else//初始化
|
||
{
|
||
motor_stop();
|
||
}
|
||
}
|
||
else if(Motor_Run == 2)//运行到近位
|
||
{
|
||
go_begin();//回到起始位
|
||
}
|
||
else if(Motor_Run == 3)//运行到远位
|
||
{
|
||
go_end();//移动至结束位
|
||
}
|
||
else//数据错误
|
||
Motor_Run = 0;
|
||
}
|
||
|
||
int t = 90;
|
||
int ADC_Fre = 0;
|
||
char ADC_Flag = 0;
|
||
uint32_t ADC_Count = 0;
|
||
uint32_t ADC_Count_array[20];//ADC变化计数数组
|
||
uint32_t ADC_Count_sum = 0; //ADC变化计数总和
|
||
/*
|
||
频率测试功能
|
||
启动时电机逐渐加速至最快
|
||
停止时电机逐渐减速至停止
|
||
*/
|
||
void frequency_test()
|
||
{
|
||
if(Run_State == 0)
|
||
{
|
||
beep_time = 0;
|
||
if(((uint16_t)(ADC_ConvertedValue[0][num_array[0]]))>=675 && ADC_Flag == 0)
|
||
{
|
||
ADC_Flag = 1;
|
||
ADC_Count++;
|
||
}
|
||
else if(((uint16_t)(ADC_ConvertedValue[0][num_array[0]]))<= 675 && ADC_Flag == 1)
|
||
ADC_Flag = 0;
|
||
if(it_500ms_flag == 1)
|
||
{
|
||
it_500ms_flag = 0;
|
||
if(t > 90)
|
||
t--;
|
||
else
|
||
{
|
||
Mode_Flag = 1;
|
||
ADC_Count = 0;
|
||
}
|
||
TIM4_PWM_Init(1999,t);
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if(((uint16_t)(ADC_ConvertedValue[0][num_array[0]]))>=675 && ADC_Flag == 0)
|
||
{
|
||
ADC_Flag = 1;
|
||
ADC_Count++;
|
||
}
|
||
else if(((uint16_t)(ADC_ConvertedValue[0][num_array[0]]))<= 675 && ADC_Flag == 1)
|
||
ADC_Flag = 0;
|
||
if(it_500ms_flag == 1)
|
||
{
|
||
it_500ms_flag = 0;
|
||
if(beep_time< 20)
|
||
{
|
||
beep_time++;
|
||
GPIO_ToggleBits(GPIOG,GPIO_Pin_0);
|
||
}
|
||
if(t < Speed)
|
||
t++;
|
||
TIM4_PWM_Init(1999,t);
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
|