#include "motor.h"
#include "app.h"

#define STEP_LIN  16		//直行程步长单位数值（脉冲数）
#define STEP_ROT  20		//角行程步长单位数值（脉冲数）
#define DATA_LEN  10		//角行程步长单位数值（脉冲数）

#define MOTOR_STOP    0		//电机停止
#define MOTOR_POS     1		//电机正向运动
#define MOTOR_REV     2		//电机反向运动
#define MOTOR_RETURN  3		//电机返回

int magnet_middle = 1240;	//磁条中点
int magnet_start = 800;		//磁条范围-起点mv
int magnet_end = 1200;		//磁条范围-终点mv
#define motor_start 200		//电机运动范围起点mv
#define motor_end 1800		//电机运动范围终点mv

int Travle_Flag = 0;            //0 直    1 角
char Motor_Run = 2;         	//0 停止  1 运行  	2 运行到起始点  3 运行到结束点
char Run_Mode = 0;          	//0 点动  1 方案一	2 方案二
unsigned int Run_Step = 0;  	//电机运行步长
unsigned int Run_Inter = 0; 	//电机运行间隔时长
unsigned int Run_Stop = 0;  	//到“结束点”后，停止时间
unsigned int Run_mm = 1;	  	//行进长度（mm）/转动角度（°）
unsigned int Run_num = 0;	  	//角行程电机转动圈数
unsigned int ct_num = 0;	  	//磁条长度

int mov_flag = 0;	//脉冲标志
int send_flag = 0;	//发送标志
int seat_flag = 0;	//位置标志
int motor_dire = 1;	//电机转动方向
int flag = 0;

int motor_direc = 1;//电机转动方向

char motor_control = 0;


//步骤
unsigned char Runmotor_step = 0;

//次数
unsigned int Runmotor_Nums = 0;

//清除电机标记
void ClrRunmotorStep(void)
{
	//步骤
	Runmotor_step = 0;
	//次数
	Runmotor_Nums = 0;	
	//脉冲标记清0
	mov_flag = 0; 	
	//发送标记清0
	send_flag = 0;
	//位置标记清0
	seat_flag = 0;
}

//处理马达运行
#define SPEED_MIN 0x00003A98
#define SPEED_NORMAL 0x00007530
#define SPEED_MAX 0x0001D4C0
int first_flag = 1;
void Deal_Motor(void)
{
	//判断直行程还是角行程
	if(Travle_Flag == 0)//直行程——电机旋转一圈，磁条水平位移5mm
	{
		motor_data[0] = 0x00;//00 直行程 01 角行程
		//判断电机停止还是运行，运行到起始位还是结束位
		if( Motor_Run == 0) 		//停止
		{
			tmc5160_operate(MOTOR_STOP,0);
			
			if( speed_max !=  SPEED_NORMAL)
			{
				speed_max = SPEED_NORMAL;				//VMAX
				TMC5160_SPIWriteInt(VMAX_ADDR, speed_max,1);
				
				speed_1 = speed_max / 2;				//V1 
				TMC5160_SPIWriteInt(V1_ADDR, speed_1,1);
			}
		}
		else if((Motor_Run == 1) && (first_flag == 0))	//运行，上电复位完成后才生效
		{
			if(Motor_Run >= 1 && Runmotor_step == 0) 
			{
                Runmotor_step = 1;
			}	
			
			//判断电机运行方式是点动还是连续，点动为方案三，方案一和方案二为连续
			if(Run_Mode == 0)//点动（方案三）
			{

				mov_step();//点动
			}
			else if(Run_Mode == 1)//连续（方案一）“步长过大可能会越过限位开关”
			{

				mov_loop1();//方案一
			}
//			else if(Run_Mode == 2)//连续（方案二）“步长过大可能会越过限位开关”
//			{

//				//磁条循环“起始点-结束点-起始点”，一定次数后停在起始点
//				mov_loop2();//方案二
//			}
			else//初始化
			{
				tmc5160_operate(MOTOR_STOP,0);
			}
		}
		else if(Motor_Run == 2)//运行到起始位
		{
			
			if( ocin1 == 0 )//判断是否到达始限位
			{
				//记录第一次复位时电机位置
				if(first_flag)
				{
					TMC5160_SPIReadInt(XACTUAL_ADDR,XA);			//读取实际位置XACTUAL
					XA_32 = Raw_32(XA);
					
					first_xa_32 = XA_32;
					first_flag = 0;
				}
				
				motor_direc = MOTOR_POS;
				Runmotor_step = 0;	
				Motor_Run = 0;
				return;
			}
			
			if( (X_ads1220 <= (motor_start + 50) ) && (X_ads1220 != 0) )
			{	
				motor_direc = MOTOR_POS;
				Runmotor_step = 0;
				Motor_Run = 0;
				//记录第一次复位时电机位置
				if(first_flag)
				{
					TMC5160_SPIReadInt(XACTUAL_ADDR,XA);			//读取实际位置XACTUAL
					XA_32 = Raw_32(XA);
					
					first_xa_32 = XA_32;
					first_flag = 0;
				}
				return;
				
			}
			
			if((ocin2 == 1) && (ocin2 == 1))						//未到达限位时，快速大步长运行
			{
				if(  speed_max != SPEED_MAX)
				{
					speed_max = SPEED_MAX;
					TMC5160_SPIWriteInt(VMAX_ADDR, speed_max,1); 	// VMAX
					
					speed_1 = speed_max / 2;						//V1 
					TMC5160_SPIWriteInt(V1_ADDR, speed_1,1);
				}
				
				tmc5160_operate(MOTOR_REV,51200*30);
			}
			
		}
		else if((Motor_Run == 3) && (first_flag == 0))				//运行到结束位
		{
			if( ocin2 == 0 )//判断是否到达终限位
			{
				motor_direc = MOTOR_REV;
				Runmotor_step = 0;	
				Motor_Run = 0;
				return;
			}
			
			if(X_ads1220 >= (motor_end - 50))
			{						
				motor_direc = MOTOR_REV;
				Runmotor_step = 0;
				Motor_Run = 0;
				return;
			}
			
			if((ocin2 == 1) && (ocin2 == 1))
			{
				if(  speed_max != SPEED_MAX)
				{
					speed_max = SPEED_MAX;
					TMC5160_SPIWriteInt(VMAX_ADDR, speed_max,1); 	// VMAX
					
					speed_1 = speed_max / 2;						//V1 
					TMC5160_SPIWriteInt(V1_ADDR, speed_1,1);
				}
				
				tmc5160_operate(MOTOR_POS,51200*30);
			}
		}
		else//数据错误
		{
			//SC_Init();
		}
	}
	else				//角行程——电机旋转一圈，磁条旋转4°
	{
//		motor_data[0] = 0x01;//00 直行程 01 角行程
//		//判断电机停止还是运行，运行到起始位还是结束位
//		if( Motor_Run == 0)//停止
//		{
//			tmc5160_operate(0,0);
//		}
//		else if(Motor_Run == 1)//运行
//		{
//			if(Motor_Run >= 1 && Runmotor_step == 0) 
//			{
//                Runmotor_step = 1;
//			}	
//			
//			//判断电机运行方式是点动还是连续，点动为方案三，方案一和方案二为连续
//			if(Run_Mode == 0)//点动（方案三）
//			{
//				motor_data[2] = 0x00;//发送时反馈的数据	
//				mov_step_ang();//电机点动运行
//			}
//			else if(Run_Mode == 1)//连续（方案一）
//			{
//				motor_data[2] = 0x01;//发送时反馈的数据
//				mov_loop1_ang();//磁条旋转一圈
//			}
//			else if(Run_Mode == 2)//连续（方案二）
//			{
//				motor_data[2] = 0x02;//发送时反馈的数据
//				if(seat_flag == 0)
//				{
//					if(GPIO_ReadPin(GPIO1,GPIO_PIN_4) == 0)	//判断是否到达限位处
//					{
//						motor_stop(); //电机停止
//						Run_mm = 0;
//						seat_flag = 1;//位置标记
//					}
//					else
//					{	
//						REV();			//反转
//						motor_start();	//运行
//						motor_mov(1);	//提供脉冲信号
//					}						
//				}
//				else
//				{
//					mov_loop2_ang();//磁条旋转一圈
//				}
//			}
//			else//初始化
//			{
//				motor_stop();
//			}
//		}
//		else if(Motor_Run == 2 || Motor_Run == 3)//运行到起始位
//		{
//			mov_begin();//回到起始位
//		}
//		else//数据错误
//			SC_Init();
	}	
}



//电机点动运行，方案三（直行程）
void mov_step(void)
{
	if( (ocin1 == 0) && (motor_direc != MOTOR_POS) )//判断是否到达始限位
	{
		motor_direc = MOTOR_POS;
		Runmotor_step = 0;	
		Motor_Run = 0;
		return;
	}
	if( (ocin2 == 0) && (motor_direc != MOTOR_REV) )//判断是否到达终限位
	{
		motor_direc = MOTOR_REV;
		Runmotor_step = 0;	
		Motor_Run = 0;
		return;
	}
	
	switch(Runmotor_step)
	{
		case 1 : //电机运行准备
			{

				Runmotor_Nums = 0;	//运行次数	
                mov_flag = 0;		 	
                Runmotor_step++; 							
			}
			break;
		case 2 : //电机运行过程						
			{
				if(Run_Step == 0)									//上位机无消息时使用下位机的控制参数Run_mm
				{
					tmc5160_operate(motor_direc,Run_mm * 10240);

				}else
				{
					tmc5160_operate(motor_direc,Run_Step * 1024);	//Runstep cm 
				}
				if(busy_flag == 0)
				{
					Runmotor_step++; 
				}
			}
			break;	
		case 3 : 
			{
				Runmotor_step++; 
			}
			break;
		case 4 : 
			{
				if(it_50ms_flag)			//检查50ms定时标志
				{
					it_50ms_flag = 0;		//定时标志清零
					Runmotor_Nums++;
				}
				if(Runmotor_Nums >= 10)		//时间
				{
					Runmotor_Nums = 0;
					Runmotor_step++;
							
					get_state();													//读取电机当前状态
					send_set_resp(0xF001, OBJ_DEVICE_ADDR, DATA_LEN, motor_data);	//数据发送
				}
			}
			break;			
		case 5 : 
			{
				Runmotor_step = 0;	
				Motor_Run = 0;   	//运行标记清除，等待下一次上位机发送命令
			}
			break;
		default :	
			{
				
			}
			break;								
	}				
}


//电机连续运行，方案一（直行程）
int stop_cnt = 0;
uint32_t motor_x_start = 0,motor_x_end = 0;
void mov_loop1(void)
{
	switch(Runmotor_step)
	{
		case 1 : //电机运行准备
			{
				
//				motor_direc = MOTOR_POS;	//由上位机控制
				Run_num = 0;
				motor_data[2] = 0x00;		//发送时反馈的数据	
				Runmotor_Nums = 0;   		//运行次数	
                mov_flag = 0;
				busy_flag = 0;
                Runmotor_step++; 					
			}
			break;
		case 2 : //电机运行过程，运行至磁条所在范围				
			{
				if( X_ads1220 >= magnet_start )			//第一次到达磁条范围的起点,以电阻尺为准
				{
					tmc5160_operate(MOTOR_STOP,0);
					while(busy_flag == 1)				//加速停止动作
					{
						tmc5160_operate(MOTOR_STOP,0);		
						
						stop_cnt++;
						if(stop_cnt > 10000) 
						{
							stop_cnt = 0;
							break;		//防止卡死
						}
						
						if(busy_flag == 0) stop_cnt = 0;
					}
					
					TMC5160_SPIReadInt(XACTUAL_ADDR,XA);		//停止后读取实际位置XACTUAL
					XA_32 = Raw_32(XA);
					
					motor_x_start = XA_32;						//记录起点时的电机位置
					
					switch(magnet_type)							//根据不同规格，计算电机运动的终点
					{
						case 25:
						{
							motor_x_end = motor_x_start + 10240*(25+6);
						}
						break;
						case 50:
						{
							motor_x_end = motor_x_start + 10240*(50+6);
						}
						break;
						case 110:
						{
							motor_x_end = motor_x_start + 10240*(110+6);
						}
						break;
						case 210:
						{
							motor_x_end = motor_x_start + 10240*(210+6);
						}
						break;
						default:
						{
							motor_x_end = 0;
						}
						break;
					}
					
					if(it_1000ms_flag)					//检查1000ms定时标志
					{
						it_1000ms_flag = 0;				//定时标志清零
						Runmotor_Nums++;
					}
					if(Runmotor_Nums >= Run_Stop)		//时间
					{
						Runmotor_step++;
						Runmotor_Nums = 0;
						get_state();										//读取电机当前状态
						send_set_resp(0xF001, OBJ_DEVICE_ADDR, DATA_LEN, motor_data);//数据发送
					}
				}
				else
				{
					if(send_flag == 0)
					{
						send_flag = 1;
//						get_state();										//读取电机当前状态
//						send_set_resp(0xF001, OBJ_DEVICE_ADDR, DATA_LEN, motor_data);//数据发送
					}
					
					if( X_ads1220 <= magnet_start - 150)					//距离磁条范围比较远时，快速运动
					{
						if(  speed_max != SPEED_MAX)
						{
							speed_max = SPEED_MAX;
							TMC5160_SPIWriteInt(VMAX_ADDR, speed_max,1); 	// VMAX
							
							speed_1 = speed_max / 2;						//V1 
							TMC5160_SPIWriteInt(V1_ADDR, speed_1,1);
						}
					}
					else													//距离磁条范围比较近时，减速至最小速度
					{
						if( speed_max !=  SPEED_MIN)
						{
							speed_max = SPEED_MIN;						//VMAX
							TMC5160_SPIWriteInt(VMAX_ADDR, speed_max,1);
							
							speed_1 = speed_max / 2;						//V1 
							TMC5160_SPIWriteInt(V1_ADDR, speed_1,1);
						}
					}
					
					tmc5160_operate( motor_direc, 51200*30 );
					
					Runmotor_step = 6;//在case 2 - 6 之间循环，2运动，6限位
				}
			}
			break;
		case 3 : 
			{
				//进入磁条范围后，以小步长进行运动，单位为mm
				if( speed_max !=  SPEED_NORMAL)
				{
					speed_max = SPEED_NORMAL;						//VMAX
					TMC5160_SPIWriteInt(VMAX_ADDR, speed_max,1);
					
					speed_1 = speed_max / 2;						//V1 
					TMC5160_SPIWriteInt(V1_ADDR, speed_1,1);
				}
				
				if(Run_Step == 0)
				{
					tmc5160_operate(motor_direc,Run_mm*10240);		//5mm-51200，1mm-10240
				}else
				{
					tmc5160_operate(motor_direc,Run_Step*1024);
				}
				if(busy_flag == 0)									//到达指定位置后释放busy_flag
				{
					Runmotor_step++; 
				}
			}
			break;
		case 4 : //延时
			{	
				if(it_50ms_flag)						//检查50ms定时标志
					{
						it_50ms_flag = 0;				//定时标志清零
						Runmotor_Nums++;
					}
				if(Runmotor_Nums >= (Run_Inter / 50))	//时间
				{
					
					Runmotor_Nums = 0;
					Runmotor_step++;
					send_flag = 0;
					
					get_state();													//读取电机当前位置
					send_set_resp(0xF001, OBJ_DEVICE_ADDR, DATA_LEN, motor_data);	//数据发送
				}
				
			}
			break;
		case 5 :
			{

				if( (ocin1 == 0) && (motor_direc != MOTOR_POS) )				//判断是否到达始限位
				{
					motor_direc = MOTOR_POS;
					Runmotor_step = 0;	
					Motor_Run = 0;
					break;
				}
				if( (ocin2 == 0) && (motor_direc != MOTOR_REV) )				//判断是否到达终限位
				{
					motor_direc = MOTOR_REV;
					Runmotor_step = 0;	
					Motor_Run = 0;
					break;
				}
				
				Runmotor_step = 3;												//超出磁条范围之前，在3-4-5之间循环
				
				TMC5160_SPIReadInt(XACTUAL_ADDR,XA);							//读取实际位置XACTUAL
				XA_32 = Raw_32(XA);
				
				if(motor_x_end != 0)											//未出现异常时以电机为准
				{
					if((XA_32 >= motor_x_end) && (motor_direc != MOTOR_REV))	//到达磁条范围终点后，停止并反向
					{

						tmc5160_operate(MOTOR_STOP,0);
						while(busy_flag == 1)				//加速停止动作
						{
							tmc5160_operate(MOTOR_STOP,0);		
							
							stop_cnt++;
							if(stop_cnt > 10000) 
							{
								stop_cnt = 0;
								break;						//防止卡死
							}
							
							if(busy_flag == 0) stop_cnt = 0;
						}
						
						if(busy_flag == 0) 
						{
							motor_direc = MOTOR_REV;		//反转
							Runmotor_Nums = 0;
							Runmotor_step = 7;
						}
					}
				}else															//出现异常时以电阻尺为准
				{
					if((X_ads1220 >= magnet_end) && (motor_direc != MOTOR_REV))	//到达磁条范围终点后，停止并反向
					{

						tmc5160_operate(MOTOR_STOP,0);
						while(busy_flag == 1)				//加速停止动作
						{
							tmc5160_operate(MOTOR_STOP,0);		
							
							stop_cnt++;
							if(stop_cnt > 10000) 
							{
								stop_cnt = 0;
								break;						//防止卡死
							}
							
							if(busy_flag == 0) stop_cnt = 0;
						}
						
						if(busy_flag == 0) 
						{
							motor_direc = MOTOR_REV;		//反转
							Runmotor_Nums = 0;
							Runmotor_step = 7;
						}
					}
				}
				
				
				
				if((XA_32 <= motor_x_start) && (motor_direc == MOTOR_REV))		//反向回到磁条范围起点后，复位
				{
					tmc5160_operate(MOTOR_STOP,0);
					while(busy_flag == 1)				//加速停止动作
					{
						tmc5160_operate(MOTOR_STOP,0);		
						
						stop_cnt++;
						if(stop_cnt > 10000) 
						{
							stop_cnt = 0;
							break;						//防止卡死
						}
						
						if(busy_flag == 0) stop_cnt = 0;
					}
					
					Motor_Run = 2;						//回电机起始点
					Runmotor_step = 0;	
				}
			
			
			}
			break;
		case 6 :
			{
				if( (ocin1 == 0) && (motor_direc != MOTOR_POS) )			//判断是否到达始限位
				{
					motor_direc = MOTOR_POS;
					Runmotor_step = 0;	
					Motor_Run = 0;
					break;
				}
				if( (ocin2 == 0) && (motor_direc != MOTOR_REV) )			//判断是否到达终限位
				{
					motor_direc = MOTOR_REV;
					Runmotor_step = 0;	
					Motor_Run = 0;
					break;
				}
				
				Runmotor_step = 2;//在case 2 - 6 之间循环，2运动，6限位
			}
			break;
		case 7 :
			{
				if(it_10ms_flag)						//检查1ms定时标志
				{
					it_10ms_flag = 0;					//定时标志清零
					Runmotor_Nums++;
				}
				if((Runmotor_Nums/100) >= Run_Stop)		//时间
				{
					Runmotor_step = 3;
					Runmotor_Nums = 0;
				}
			}
			break;
		default :	
			{
				
			}
			break;								
	}				
}

uint32_t V_data = 0;
signed int _data = 0,vol_data = 0;
char start_check = 0;
uint16_t send_xa = 0;
void get_state(void)
{
	motor_data[1] = Motor_Run;
	motor_data[2] = Run_Mode;
	
	if(rx_data2[start_check] != 0x05)
	{
		start_check = (start_check + 1)*(start_check <5);								//定位起始帧
	}
	else
	{
		motor_data[4] = rx_data2[ (start_check + 1) - 6*( (start_check + 1) > 5) ];  	//根据起始帧所在位置，记录磁感应模块的反馈值
		motor_data[5] = rx_data2[ (start_check + 2) - 6*( (start_check + 2) > 5) ];
		motor_data[6] = rx_data2[ (start_check + 3) - 6*( (start_check + 3) > 5) ];
		motor_data[7] = rx_data2[ (start_check + 4) - 6*( (start_check + 4) > 5) ];
	}
	
	
	/*电阻尺读数*/
//	motor_data[8] = ((uint16_t)(X_ads1220*10) & 0xff00)>> 8;	//磁条长度高8位
//	motor_data[9] = (uint16_t)(X_ads1220*10) & 0x00ff;			//磁条长度低8位
	
	/*电机内部编码*/
	TMC5160_SPIReadInt(XACTUAL_ADDR,XA);					//读取实际位置XACTUAL
	XA_32 = Raw_32(XA);
	send_xa = ((XA_32 - first_xa_32)  * 100) / 10240;
	motor_data[8]  = ((uint16_t)send_xa & 0xff00)>> 8;	//磁条长度高8位
	motor_data[9]  = ((uint16_t)send_xa & 0x00ff);		//磁条长度低8位
	
	if( ocin1 == 0 )//判断是否到达始限位
		motor_data[3] = 1;
	else if(ocin2 == 0)
		motor_data[3] = 2;
	else
		motor_data[3] = motor_direc + 2;
}

void get_magnetv(void)
{
	V_data = motor_data[4]*0xffffff + motor_data[5]*0xffff + motor_data[6]*0xff + motor_data[7];
	//ADS数据转换为实际电压值
	if(V_data & 0x00800000)
	{
		_data = 0xffffffff - V_data;
		vol_data = (-2.5 / 0x7fffff) * _data*1000; 
	}
	else
	{
		_data = V_data;
		vol_data = (2.5 / 0x7fffff) * _data*1000;
	}
}