裴昌跃 / AGVkunshantaiguang · Files

#include "includes.h"
#include "hardware.h"
#include "all_value.h"

u8 handnum_t=0;
/******************************************************************
定时器中断服务程序
*******************************************************************/
//定时器3中断服务函数
void TIM3_IRQHandler(void)
{
	static int time1=0;
	static int time2=0;
	static int time3=0;
	static int time4=0;
	static int time5=0;
	static int time6=0;
	
	if(TIM_GetITStatus(TIM3,TIM_IT_Update)==SET) //溢出中断led
	{
		time1++;time2++;time3++;time4++;time5++;time6++;
		get_power_T++;//

		if(countTimeCut>0){countTimeCut--;}//
		if(countTimeCut==0)
		{
			TcpFlag=1;
		}else TcpFlag=0;
//		{
//			if(time1<50){LEDG=1;LEDY=1;LEDW=1;}else if(time1<2*50){LEDG=0;LEDY=0;LEDW=0;}else time1=0;	//agv掉线三灯齐闪
//		}else
		{
			if(Auto==0)   	{LEDG=1;}else																	//手动模式绿灯常亮
			if(TaskNum==0)	{if(time2<1000)LEDG=1;else if(time2<2*1000)LEDG=0;else time2=0;}else			//自动空闲绿灯慢闪
			if(TaskNum!=0)	{if(time3<300) LEDG=1;else if(time3<2*300) LEDG=0;else time3=0;}				//自动忙碌绿灯快闪
			
			if(radarstop)   {if(time4<100) LEDY=1;else if(time4<2*100) LEDY=0;else time4=0;}else        	//有障碍物黄灯快闪
			if(voltagealarm){if(time5<300) LEDY=1;else if(time5<2*300) LEDY=0;else time5=0;}else LEDY=0;	//电量过低黄灯慢闪
			
			if(flagcode1||isShelfBe)    {if(time6<300) LEDW=1;else if(time6<2*300) LEDW=0;else time6=0;}else LEDW=0;	//有码有磁条白灯快闪
		}
	}
	TIM_ClearITPendingBit(TIM3,TIM_IT_Update);  //清除中断标志位
}
//定时器4中断服务函数
void TIM4_IRQHandler(void)
{
	static int time1=0;//
	static int time2=0;//
	static int time3=0;//
//	static int time4=0;//
	static int time5=0;//
	static int time6=0;//
	static int time7=0;//
	static int time8=0;//
	static int time9=0;//
	static int time10=0;
	static u8 modenew=0;
	static u8 modeold=0;
	static u8 agv_start_save=1;
	
	if(TIM_GetITStatus(TIM4,TIM_IT_Update)==SET) //溢出中断lvbo
	{		
		/*急停信号滤波处理*/
		if(Stopnow==1){if(time1<INT32_MAX)time1++;}else{time1=0;Stop=0;}//急停滤波
		if(time1>100){Stop=1;time1=0;}
		
		/*模式开关信号滤波处理*/
		modenew=Modenow;
		if(modenew==1){if(time2<INT32_MAX)time2++;}
		if(modenew<modeold)//下降沿
		{
			if(time2>300)//滤波
			{
				if(time2<2000)Auto=!Auto;else//2s内模式信号
				if(time2>5000&&time2<10000){if(Auto==0)initilize_code=1;}else //5~10s高位复位
				if(time2>=10000){if(Auto==0)initilize_code=2;}//>15s下降复位
			}
			time2=0;
		}
		modeold=modenew;
		
		if(Stop)Auto=0;
		
		/*有码标志信号滤波处理*/
		if(flagcode)time3=20;
		if(time3==0){flagcode1=0;} else{time3--;flagcode1=1;}
		
		/*防撞检测信息处理:
		 *行走的时候开启防撞，不行走关闭防撞
		 *tim一检测到障碍物，滤波30ms后有障碍物标志直接置1
		 *tim检测障碍物消失，等待2秒钟 有障碍物标志直接置0
		 *tim暂停时间过长，等待2分钟，报错
		 */
		if(run_start_flag||arc_start_flag)
		{
			if(radararea!=0){if(time5<INT32_MAX)time5++;time6=0;}else{if(time6<INT32_MAX)time6++;time5=0;}
			if(time5>30)  radarstop=1;
			if(time6>5000)radarstop=0;
			
			if(radarstop==1&&radararea==1){if(time7<INT32_MAX)time7++;}else {tim_err=0;time7=0;}
			if(time7>1000*60*2)tim_err=1;
		}else 
		{
			time5=0;
			time6=0;
			tim_err=0;
			radarstop=0;
		}
		
		if(Charge_on==1){if(charge_T<INT32_MAX)charge_T++;}else charge_T=0;
		
		/*开机保护，AGV开机后5分中内不会自动断电*/
		/*出现故障后1h自动断电
		 *或者电量过低自动断电
		 *自动断电出现在  1、电量低&&自动&&非充电&&没任务  2、自动&&报错&&1小时后
		 */

		if(agv_start_save&&Auto==1){if(time8<INT32_MAX)time8++;}
		if(time8>1000*60*5)//1ms*1000*60*5
		{
			time8=0;
			agv_start_save=0;
		}
		if(agv_start_save==0)
		{
			if(Ecode[0]&&Auto){if(time9<INT32_MAX)time9++; }else time9=0;  //出现故障
			if((time9>(1000*60*60)))Agv_close=1;			//
			
			if(Voltage<40)//电量过低   自动空闲断电time35
			{
				if(time10<INT32_MAX)time10++;
			}
			if(Voltage>42||Charge_on||TaskNum||Auto==0)time10=0;
			
//			if(time10>(1000*60*4+1000*55))Autoclose=1;  //上报上位机自动断电标志
			if(time10>(1000*60*5))Agv_close=1;	//电量过低，非充电，空闲，自动模式5min之后，自动断电
		}	
//		if(time26==0)Handnum=0;else{time26--;Handnum=handnum_t;}
		if(TCP_Handnum_t==0){TCP_Handnum=0;}else{TCP_Handnum_t--;}//上位机ui手动控制
	}
	TIM_ClearITPendingBit(TIM4,TIM_IT_Update);  //清除中断标志位
}

//定时器5中断服务函数
double Distancesurplus=0;
double run_speedmax=0;   //最高速
void TIM5_IRQHandler(void)
{
	static double target_v_old=0;
	static double target_v_new=0;
	static double Distancesurplusold=0;
//	static u8 radarstop_old=0;
	static u8 time1=0;
	static int time2=0;
	double runAc=0.15;
	double runDe=0.15;
	double runStopSpeed = 0.01;
	if(TIM_GetITStatus(TIM5,TIM_IT_Update)==SET) //溢出中断tcp
	{
		if(run_start_flag==1)
		{
			/*最高速度的选择*/
			Distancesurplus=task1.distance-Distancerun;	//计算剩余距离
			if(Distancesurplus<0)Distancesurplus=0;		//剩余距离不可小于0
			if(Distancesurplus>Distancesurplusold+100)	//行走距离更新，10cm防止有误（酌情的添加防撞感应的速度恢复的判断）防撞的障碍物消失||radarstop<radarstop_old
			{
				if(pt[get_startstopcode_dir[3]-1]==18||pt[0]==18)//电梯点
				{
					run_speedmax=SPEEDMAX_SELECTOR(Distancesurplus/1000,runspeed,runStopSpeed,runAc,runDe,0.15,0.4,0.2,0.1);//选择合适最大速
					ACCELERATE_DECELERATE_STATE.de_dis=0.5*fabs(run_speedmax-runStopSpeed)*(run_speedmax+runStopSpeed)/runDe;		//计算该最高速的减速距离
				}else
				if(pt[get_startstopcode_dir[3]-1]==20||pt[0]==20)//提升机
				{
					run_speedmax=SPEEDMAX_SELECTOR(Distancesurplus/1000,runspeed,runStopSpeed,runAc,runDe,0.15,0.4,0.2,0.1);//选择合适最大速
					ACCELERATE_DECELERATE_STATE.de_dis=0.5*fabs(run_speedmax-runStopSpeed)*(run_speedmax+runStopSpeed)/runDe;		//计算该最高速的减速距离
				}else
				{
					run_speedmax=SPEEDMAX_SELECTOR(Distancesurplus/1000,runspeed,runStopSpeed,runAc,runDe,0.15,dataRunSpeedMax1,dataRunSpeedMax2,dataRunSpeedMax3);//选择合适最大速
					ACCELERATE_DECELERATE_STATE.de_dis=0.5*fabs(run_speedmax-runStopSpeed)*(run_speedmax+runStopSpeed)/runDe;		//计算该最高速的减速距离
				}
			}
			Distancesurplusold=Distancesurplus;
//			radarstop_old=radarstop;
			
			/*计算目标速度，及其配置*/
			if(Stop==1||Safe==1)
			{
				ACCELERATE_DECELERATE_STATE.target_v=0;
				ACCELERATE_DECELERATE_STATE.t=0.5;
			}else
			if(Distancerun>=task1.distance+300)
			{
				if(flagcode1==0){if(time2<INT32_MAX)time2++;}else {code_err=0;time2=0;}
				if(time2>1000*15)code_err=1;

				ACCELERATE_DECELERATE_STATE.target_v=0;
				ACCELERATE_DECELERATE_STATE.t=0.5;
			}else
			if(Distancerun<task1.distance+300)//速度控制
			{
				code_err=0;time2=0;
				if(radarstop==1)
				{
					if(radararea==1){ACCELERATE_DECELERATE_STATE.target_v=0;   ACCELERATE_DECELERATE_STATE.t=0.5;}else
					if(radararea==2){ACCELERATE_DECELERATE_STATE.target_v=0.03;ACCELERATE_DECELERATE_STATE.t=1.0;}else
					if(radararea==3){ACCELERATE_DECELERATE_STATE.target_v=0.03;ACCELERATE_DECELERATE_STATE.t=1.0;} 
				}else
				if(radarstop==0)
				{
					if(Distancerun<task1.distance-(1000*ACCELERATE_DECELERATE_STATE.de_dis+runkk*run_speedmax))
					{
						ACCELERATE_DECELERATE_STATE.target_v=run_speedmax;
						ACCELERATE_DECELERATE_STATE.t=run_speedmax/runAc;//配置此次速度变化的时间   s秒
					}else
					{
						ACCELERATE_DECELERATE_STATE.target_v=runStopSpeed;
						ACCELERATE_DECELERATE_STATE.t=run_speedmax/runDe;//配置此次速度变化的时间
					}
				}
			}			
			
			/*速度的曲线*/
			time1=!time1;
			target_v_new=ACCELERATE_DECELERATE_STATE.target_v;
			if(target_v_new!=target_v_old)
			{
				ACCELERATE_DECELERATE_RESET();
				ACCELERATE_DECELERATE_STATE.initial_v=runspeed;
				ACCELERATE_DECELERATE_SET(&ACCELERATE_DECELERATE_STATE,0);
				time1=0;
			}
			target_v_old=target_v_new;
			
			run_speed=ACCELERATE_DECELERATE_SPEED(&ACCELERATE_DECELERATE_STATE,time1,0.002);
		}else
		{
			ACCELERATE_DECELERATE_RESET();
			ACCELERATE_DECELERATE_STATE.de_dis=0;
		//	ACCELERATE_DECELERATE_STATE.target_v=0;
		//	ACCELERATE_DECELERATE_STATE.t=0;

			target_v_old=0;
			target_v_new=0;
			Distancesurplusold=0;
			run_speedmax=0;
			code_err=0;time2=0;
		}
		runspeed=run_speed;

	}
	TIM_ClearITPendingBit(TIM5,TIM_IT_Update);   //清除中断标志位
}

//定时器7中断服务函数
void TIM7_IRQHandler(void)
{
	if(TIM_GetITStatus(TIM7,TIM_IT_Update)==SET) //溢出中断
	{
//		if(radar_de_flag==1){radar_speed=runspeed*1000;radar_de_flag=0;}//雷达减速控制
//		if(radar_speed>0)radar_speed-=(radar_de/100);else radar_speed=0;
	}
	TIM_ClearITPendingBit(TIM7,TIM_IT_Update);  //清除中断标志位
}

/******************************************************************
串口中断服务程序
*******************************************************************/
u8 RS485_RX_BUF[15];  	
u8 RS485_RX_CNT=0;   
void USART6_IRQHandler(void)//RS485接收中断
{
	u8 res;
	OSIntEnter();    
	if(USART_GetITStatus(USART6, USART_IT_RXNE) != RESET)  
	{
		res=USART_ReceiveData(USART6); 
		if(RS485_RX_CNT<15)
		{
			RS485_RX_BUF[RS485_RX_CNT]=res;		//记录接收到的值
			if(RS485_RX_BUF[0]==0xAA){RS485_RX_CNT++;flagcode4=1;}//接收数据增加1 
		} 		
	} 
	USART_ClearITPendingBit(USART6,USART_IT_RXNE);
	OSIntExit();  											 
} 
void RS485_Receive_Data1(u8 *buf)
{
	u8 rxlen=15;
	u8 i=0;
	if(rxlen==RS485_RX_CNT)
	{
		for(i=0;i<rxlen;i++)
		{
			buf[i]=RS485_RX_BUF[i];	
		}			
		RS485_RX_CNT=0;
	}
}
u8 UART2_RX_BUF[14];
u8 UART2_RX_CNT=0;
void USART2_IRQHandler(void)//串口2中断服务程序
{
	u8 res;
	OSIntEnter();    
	if(USART_GetITStatus(USART2, USART_IT_RXNE) != RESET)  
	{
		res=USART_ReceiveData(USART2); 
		if(UART2_RX_CNT<14)
		{
			UART2_RX_BUF[UART2_RX_CNT]=res;		//记录接收到的值
			if(UART2_RX_BUF[0]==0x68)UART2_RX_CNT++;//接收数据增加1
		} 		
	} 
	USART_ClearITPendingBit(USART6,USART_IT_RXNE);
	OSIntExit();  											 
} 
void RS232_Receive_Data(u8 *buf,u8 *len)
{
	u8 rxlen=14;
	u8 i=0;
	if(rxlen==UART2_RX_CNT)
	{
		for(i=0;i<rxlen;i++)
		{
			buf[i]=UART2_RX_BUF[i];
		}
		UART2_RX_CNT=0;		
	}
}

u8 UART5_RX_BUF[10];
u8 UART5_RX_CNT=0;
void UART5_IRQHandler(void)//串口5中断服务程序
{
	u8 res;
	OSIntEnter();    
	if(USART_GetITStatus(UART5, USART_IT_RXNE) != RESET)
	{
		res=USART_ReceiveData(UART5); 
		if(res==0)TTL_flag=1;else TTL_flag=0;
		if(UART5_RX_CNT<sizeof(UART5_RX_BUF))
		{
			UART5_RX_BUF[UART5_RX_CNT]=res;		//记录接收到的值
			if(UART5_RX_BUF[0]==0xAA){UART5_RX_CNT++;}//接收数据增加1 
		} 
	}

	USART_ClearITPendingBit(UART5,USART_IT_RXNE);
	OSIntExit();

}

void UART5_Receive_Data(u8 *buf)
{
	u8 rxlen=10;
	u8 i=0;
	if(rxlen==UART5_RX_CNT)
	{
		for(i=0;i<rxlen;i++)
		{
			buf[i]=UART5_RX_BUF[i];
		}
		UART5_RX_CNT=0;
	}
}

/******************************************************************
CAN中断服务程序
*******************************************************************/
Motor Motor1;
Motor Motor2;
Motor Motor3;
Motor Motor4;
float BatteryV=0;
float BatteryI=0;
u8 BatteryAh=0;
u8 BatteryT=0;
u8 BatteryCharge=0;
u8 Batterycnt=0;

#if CAN1_RX0_INT_ENABLE	//使能RX0中断
//中断服务函数			    
void CAN1_RX0_IRQHandler(void)
{
	CanRxMsg RxMessage;
	double delta;
	u8 i=0;
	OSIntEnter();
	CAN_Receive(CAN1, 0, &RxMessage);
	if(RxMessage.StdId==0x581)//收到1号电机数据
	{
//			if(RxMessage.Data[0]==0x43&&RxMessage.Data[1]==0x6c&&RxMessage.Data[2]==0x60&&RxMessage.Data[3]==0x00)//接收到速度数据
//			{
//				for(i=7;i>=4;i--)
//				{
//					Motor1.speed=(Motor1.speed<<8)+RxMessage.Data[i];
//				}
//				Motor1.speed=-Motor1.speed;
//				Motor1.Velocity=Motor1.speed*((double)dataWheelDiameter*PI)/(512*10000.0/1875)/(double)dataWheelReductionRatio/60;
//			}else 			
		if(RxMessage.Data[0]==0x43&&RxMessage.Data[1]==0x63&&RxMessage.Data[2]==0x60&&RxMessage.Data[3]==0x00)//接收到位置信息
		{
			Motor1.local_old=Motor1.local;
			for(i=7;i>=4;i--)
			{
				Motor1.local=(Motor1.local<<8)+RxMessage.Data[i];
			}
			Motor1.local=-Motor1.local;
			delta=(double)Motor1.local-(double)Motor1.local_old;
			if(delta<-2147483648.0)//正向溢出
			{
				Motor1.local_delta+=1;
			}
			else if(delta>2147483648.0)
			{
				Motor1.local_delta-=1;
			}
			Motor1.Position=((double)Motor1.local+Motor1.local_delta*2147483648.0*2)*((double)dataWheelDiameter*PI)/10000.0/(double)dataWheelReductionRatio;//右轮
		}
//		else if(RxMessage.Data[0]==0x4b&&RxMessage.Data[1]==0x78&&RxMessage.Data[2]==0x60&&RxMessage.Data[3]==0x00)
//		{
//			Motor1.current=((RxMessage.Data[5]<<8)+RxMessage.Data[4]);///45.5;
//			Motor1.Current=(double)Motor1.current/36.198;
//		} 
	}
	
	else	if(RxMessage.StdId==0x582)//收到2号电机数据
	{
		if(RxMessage.Data[0]==0x43&&RxMessage.Data[1]==0x63&&RxMessage.Data[2]==0x60&&RxMessage.Data[3]==0x00)//接收到位置信息
		{
			Motor2.local_old=Motor2.local;
			for(i=7;i>=4;i--)
			{
				Motor2.local=(Motor2.local<<8)+RxMessage.Data[i];
			}
			delta=(double)Motor2.local-(double)Motor2.local_old;
			
			
			if(delta<-2147483648.0)//正向溢出
			{
				Motor2.local_delta+=1;
			}
			else if(delta>2147483648.0)
			{
				Motor2.local_delta-=1;
			}
			Motor2.Position=(Motor2.local+Motor2.local_delta*2147483648.0*2)*((double)dataWheelDiameter*PI)/10000.0/(double)dataWheelReductionRatio;//左轮
		}
//		else if(RxMessage.Data[0]==0x4b&&RxMessage.Data[1]==0x78&&RxMessage.Data[2]==0x60&&RxMessage.Data[3]==0x00)
//		{
//			Motor2.current=((RxMessage.Data[5]<<8)+RxMessage.Data[4]);///45.5;
//			Motor2.Current=(double)Motor2.current/36.198;
//		} 
		
	}
	
	else if(RxMessage.StdId==0x583)//收到3号电机数据
	{

		if(RxMessage.Data[0]==0x43&&RxMessage.Data[1]==0x63&&RxMessage.Data[2]==0x60&&RxMessage.Data[3]==0x00)//接收到位置信息
		{
			for(i=7;i>=4;i--)
			{
				Motor3.local=(Motor3.local<<8)+RxMessage.Data[i];
			}
			Motor3.local=-Motor3.local;
			Motor3.Position=(Motor3.local-origin)*(double)dataThreadDis/10000.0/dataUpDownReductionRatio;//20.48
		}

//		else if(RxMessage.Data[0]==0x4b&&RxMessage.Data[1]==0x78&&RxMessage.Data[2]==0x60&&RxMessage.Data[3]==0x00)
//		{
//			Motor3.current=((RxMessage.Data[5]<<8)+RxMessage.Data[4]);///45.5;
//			Motor3.Current=(double)Motor3.current/36.198;
//		} 
	}
	
	else if(RxMessage.StdId==0x584)//收到4号电机数据
	{
		if(RxMessage.Data[0]==0x43&&RxMessage.Data[1]==0x63&&RxMessage.Data[2]==0x60&&RxMessage.Data[3]==0x00)//接收到位置信息
		{
			Motor4.local_old=Motor4.local;
			for(i=7;i>=4;i--) 
			{
				Motor4.local=(Motor4.local<<8)+RxMessage.Data[i];
			}
			delta=(double)Motor4.local-(double)Motor4.local_old;
			
			if(delta<-2147483648.0)//正向溢出
			{
				Motor4.local_delta+=1;
			}
			else if(delta>2147483648.0)
			{
				Motor4.local_delta-=1;
			}
//			if(dataTrayReductionRatio<0||dataTrayReductionRatio>500)dataTrayReductionRatio=256.0;
			Motor4.Position=(Motor4.local+Motor4.local_delta*2147483648.0*2)*(360.0)/10000/dataTrayReductionRatio;
		}
//		else if(RxMessage.Data[0]==0x4b&&RxMessage.Data[1]==0x78&&RxMessage.Data[2]==0x60&&RxMessage.Data[3]==0x00)
//		{
//			Motor4.current=((RxMessage.Data[5]<<8)+RxMessage.Data[4]);///45.5;Current
//			Motor4.Current=(double)Motor4.current/60.33;
//		} 
	}
	else
	if(RxMessage.StdId==0xbb)//接受聚和源电池的数据
	{
		BatteryV=((RxMessage.Data[0]<<8)+(RxMessage.Data[1]))*0.1f;
		BatteryI=((RxMessage.Data[2]<<8)+(RxMessage.Data[3]))*0.1f;
		BatteryAh=RxMessage.Data[4];
		BatteryT=RxMessage.Data[5]-40;//pianyi-40
		BatteryCharge=RxMessage.Data[6];//1Chongdian   0meichongdian
		Batterycnt=RxMessage.Data[7];
	}
	
	OSIntExit(); 
}
#endif