//Enbart servo reset

bool ResetNow = exec.GetLED(ResetLED);
string Line;

if (FirstRun994 && !ResetNow)
{
  while (!exec.GetLED(ResetLED))
    Thread.Sleep(10);
  ResetNow = exec.GetLED(ResetLED);
}

FirstRun994 = false;


bool ResetKlar = false;

int startUpDelay = 75;
bool ResetOK = false;
bool servoErrorOutSet = false;
int servoErrorLEDX = 11; //Servo X
int servoErrorLEDY = 13; //Servo Y
int servoErrorLEDZ = 15; //Servo Z
int servoErrorLEDA = 12; //Servo A

int servoErrorResetPin = 4;
int servoErrorResetPort = 3;
int counterResetDelay = 0;
int ResetDelay = 100;

//Init outputs
exec.Clroutpin(servoErrorResetPort, servoErrorResetPin);//Port / pin Reset servo larm

		

Console.Write("994 Reset servo Started");

while(RunMacroPumpServoReset && !FirstRun994)
{
	Thread.Sleep(100);


	if(!exec.GetLED(25) && !ResetOK) //reset servo
	{
		Console.Write("--------Setting port 2 pin 17----------");
		exec.Setoutpin(2, 17);//Port / pin Reset servo larm
		Console.Write("----------------------");
		exec.Setoutpin(servoErrorResetPort, servoErrorResetPin);//Port / pin Reset servo larm
		ResetOK = true;
		exec.AddStatusmessage("Servo alarm reseting....");
		servoErrorOutSet = true;
		Console.Write("----------------------");
		Console.Write("Setting port " + servoErrorResetPort + " and pin " + servoErrorResetPin);
		Console.Write("Reset servo");
		Console.Write("ResetOK = " + ResetOK);
		Console.Write("servoErrorOutSet = " + servoErrorOutSet);
	}
	
	if(exec.GetLED(25) && ResetOK) //In reset mode, reseting variables
	{
		Console.Write("--------Clearing port 2 pin 17----------");
		exec.Clroutpin(2, 17);//Port / pin Reset servo larm
		exec.Clroutpin(servoErrorResetPort, servoErrorResetPin);//Port / pin Reset servo larm
		counterResetDelay = 0;
		ResetOK = false;
		servoErrorOutSet = false;
		Console.Write("----------------------");
		Console.Write("In reset mode, reseting variables");
		Console.Write("ResetOK = " + ResetOK);
		Console.Write("servoErrorOutSet = " + servoErrorOutSet);
	
	}
	
	if(ResetOK && servoErrorOutSet) //Reset servo clear alarm output
	{	
		counterResetDelay++;
		
		if(counterResetDelay > ResetDelay)
		{
			exec.Clroutpin(servoErrorResetPort, servoErrorResetPin);//Port / pin Reset servo larm
			exec.AddStatusmessage("Servo alarm reset OK");
			servoErrorOutSet = false;
			Console.Write("----------------------");
			Console.Write("Reset servo clear alarm output");
			Console.Write("ResetOK = " + ResetOK);
			Console.Write("servoErrorOutSet = " + servoErrorOutSet);
			counterResetDelay = 0;
		}
	}
	
	
	if(ResetOK && servoErrorOutSet && !exec.GetLED(servoErrorLEDX)&& !exec.GetLED(servoErrorLEDY)&& !exec.GetLED(servoErrorLEDA) && !exec.GetLED(servoErrorLEDZ)) //Reset servo clear alarm output
	{	
		
		exec.Clroutpin(servoErrorResetPort, servoErrorResetPin);//Port / pin Reset servo larm
	
		exec.AddStatusmessage("Servo alarm reset OK");
		servoErrorOutSet = false;
		Console.Write("----------------------");
		Console.Write("Reset servo clear alarm output");
		Console.Write("ResetOK = " + ResetOK);
		Console.Write("servoErrorOutSet = " + servoErrorOutSet);
	
	}
	
		
}

	

#Events



static bool RunMacroPumpServoReset = true;

const int ResetLED = 25;
static bool FirstRun994 = true;
 

