www.pudn.com > TMS320F2812Sci.rar > mainloop.c

 
#include "DSP28_Device.h" 
 
#define SENDCODE 1 
 
int DisplayType = AZ; 
 
struct PidData AzPidCtrl; 
struct PidData ElPidCtrl; 
 
int CtrlEnFlag = 0; 
 
int az_cmd =0 , el_cmd = 0; 
 
int CurrentAxis = AZ; 
 
int FaultFlag = 0; 
 
void MainLoop(void) 
{ 
	unsigned int di_val; 
	int az_code, el_code; 
	int az_cmd_pos, el_cmd_pos; 
 
	//Read Resover to Digital Convert 
	az_code = ReadRDC(AZ); 
	el_code = ReadRDC(EL); 
	 
	//Send Azimuth and Elevation Encode to Center computer 
#if SENDCODE 
	SendEncoder(AZ, az_code); 
	SendEncoder(EL, el_code); 
#endif 
	 
	di_val = ReadDigitalInput();	//Read Digital Input 
	ProcessDigInput(di_val); 
			 
	//Read Command from Center computer 
	RecvCmd(); 
	if(ReadAzCmd(&az_cmd_pos) == 1) 
	{ 
		az_cmd = (az_code - az_cmd_pos)/11;	//Get Servo Control Command 
	} 
	if(ReadElCmd(&el_cmd_pos) == 1) 
	{ 
		el_cmd = (el_code - el_cmd_pos)/11;//Get Servo Control Command 
	}	 
	 
	if(CurrentAxis != AZ) 
	{ 
		CheckRangeSendCmd(AZ, di_val, az_cmd);//check the output range and send command to controller 
		CurrentAxis = AZ; 
	} 
	else  
	{ 
		CurrentAxis = EL; 
		CheckRangeSendCmd(EL, di_val, el_cmd); 
	} 
	 
	//Process Key Input and send affirm to center computer 
	if(KeyInput() == 1) 
	{ 
		SciSend(PRIM, 0xDD); 
		SendEncoder(PRIM, AzZeroPos); 
		SendEncoder(PRIM, ElZeroPos); 
	} 
 
	//Display 
	if(DisplayType == AZ) 
	{ 
		Display(AZ, az_code); 
	} 
	if(DisplayType == EL) 
	{ 
		Display(EL, el_code); 
	} 
	if(DisplayType == 3) 
	{ 
		Display(3, AzZeroPos); 
	} 
	if(DisplayType == 4) 
	{ 
		Display(4, ElZeroPos); 
	} 
	if(DisplayType == 5) 
	{ 
		if(BreakFlag == 1) 
			Display(5, 11111); 
		else 
			Display(5, 0); 
	} 
	if(DisplayType == 0) 
	{ 
		Display(0, PressedKeyValue); 
	} 
 
	 
	//Build In Test 
	if(RecvCtrlSts() == -1) 
	{ 
		if(AzCtrlErrCode != 0) 
		{ 
			if(CtrlEnFlag == 1) 
			{ 
				StopController(); 
			} 
			FaultFlag = 1; 
			//Send Fault Code to center computer		 
			SciSend(PRIM, 0xFA); 
		} 
		if(ElCtrlErrCode != 0) 
		{ 
			if(CtrlEnFlag == 1) 
			{ 
				StopController(); 
			} 
			FaultFlag = 1; 
			//Send Fault Code to center computer 
			SciSend(PRIM, 0xFE); 
		} 
	} 
 
	 
} 
 
void StartController(void) 
{ 
	SendCommand(1);		//select elevation controller  
	SendCommand(6);		//enable controller 
	SendCommand(0);		//select azimuth controller 
	SendCommand(6);		//enable controller 
	CtrlEnFlag = 1; 
	CurrentController = AZ; 
} 
 
void StopController(void) 
{ 
	SendCommand(1);		//select elevation controller  
	SendCommand(5);		//disable controller 
	SendCommand(0);		//select azimuth controller 
	SendCommand(5);		//disable controller 
	CtrlEnFlag = 0; 
	CurrentController = AZ; 
} 
 
int CheckRangeSendCmd(int axis, unsigned di_value, int output_val) 
{ 
	int cmd_val; 
	 
	cmd_val =  -500;//output_val; 
//	Final Conditions 
//	if(((di_value & AZ_LOCK) != 0) || ((di_value & AZ_HAND) != 0) 
//	   || ((di_value & EL_HAND) != 0) || ((di_value & AZ_EN) == 0) 
//	   || ((di_value & EL_EN) == 0) || (di_value & ENABLED) != 0) 
// Debug Conditions	   	 
	if(((di_value & AZ_LOCK) != 0) || ((di_value & AZ_HAND) != 0) 
	   || ((di_value & EL_HAND) != 0) || ((di_value & AZ_EN) != 0) 
	   || ((di_value & EL_EN) != 0) || ((di_value & ENABLED) !=0) 
	   || (BreakFlag == 1)) 
	{ 
		if(CtrlEnFlag == 1) 
			StopController(); 
	} 
	else 
	{ 
		if((CtrlEnFlag == 0) && (FaultFlag == 0) && (BreakFlag == 0)) 
			StartController();	 
		if((axis == AZ) && (CtrlEnFlag == 1)) 
		{ 
			if(((di_value & LEFT_DETENT) != 0) && (cmd_val < 0)) 
			{ 
				cmd_val = 0; 
			} 
			if(((di_value & RIGHT_DETENT) != 0) && (cmd_val > 0)) 
			{ 
				cmd_val = 0; 
			} 
			CurrentController = AZ; 
			SendCommand(0);	//Select Azimuth controller 
			SendJogCmd(cmd_val); 
		} 
		if((axis == EL) && (CtrlEnFlag == 1)) 
		{ 
			if(((di_value & DOWN_DETENT) != 0) && (cmd_val < 0)) 
			{ 
				cmd_val = 0; 
			} 
			if(((di_value & UP_DETENT) != 0) && (cmd_val > 0)) 
			{ 
				cmd_val = 0; 
			} 
			CurrentController = EL; 
			SendCommand(1);	//Select elevation controller 
			SendJogCmd(cmd_val); 
		} 
	} 
	return 0;		 
} 
 
/*==================================== 
	no more 
====================================*/