#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);
		}
	}
}