#include <csl_tmr.h>
#include "environment.h"
#include "timer.h"
#include "port.h"
#include "system_setup.h"
#include "servo_motor.h"
#include "machineControl.h"

#define	SERVO_MOTOR_TIMER_INTERVAL2			25000000

#define	SERVO_ON_WAITING_TIME				500		// 500ms
#define	SERVO_OFF_WAITING_TIME				500		// 500ms

#define	SERVO_MOTOR_1_ENCODER_INDEX			6
#define	SERVO_MOTOR_2_ENCODER_INDEX			7
#define	SERVO_MOTOR_3_ENCODER_INDEX			8
#define	SERVO_MOTOR_4_ENCODER_INDEX			9
#define	SERVO_MOTOR_5_ENCODER_INDEX			10

TServoMotorInfo	ServoMotorInfo[SERVO_MOTOR_COUNT];

TServoMotorData	ServoMotorSetupData[MAX_SERVO_MOTOR_COUNT];

int Moter3SpeedChangeOn;
int Moter3Speed;
int moterPortonoff=0;
int Timer10usCount=0;

void Servo_Motor_ServoOff(int motorIndex);

void ServoOn(int motorIndex)
{
	PORT_ModifyOutPort(ServoMotorInfo[motorIndex].ServoOnPortIndex,	1);
}

void ServoOff(int motorIndex)
{
	PORT_ModifyOutPort(ServoMotorInfo[motorIndex].ServoOnPortIndex,	0);

	if(MachineSetupData.MachineType == MACHINE_TYPE_SELMA_ADVANCED || MachineSetupData.MachineType == MACHINE_TYPE_LIMA_ADVANCED)
	{	
		if(motorIndex==2)
		{
			Moter3SpeedChangeOn = 0;
		}
	}
}

void Servo_Motor_Init(void)
{
	int	motorIndex;
	int checkServoMotorCount = SERVO_MOTOR_COUNT;	
	
	if(MachineSetupData.MachineType == MACHINE_TYPE_SELMA_ADVANCED || MachineSetupData.MachineType == MACHINE_TYPE_LIMA_ADVANCED)
	{
		checkServoMotorCount = SERVO_MOTOR_COUNT;
	}
	else
	{
		checkServoMotorCount = SERVO_MOTOR_COUNT - 1;
	}	

	for	(motorIndex	= 0; motorIndex	< checkServoMotorCount; motorIndex++)
	{
		ServoMotorInfo[motorIndex].PrevSpeed = SERVO_MOTOR_MAX_SPEED_VALUE;
		ServoMotorInfo[motorIndex].CurrentSpeed	= SERVO_MOTOR_MAX_SPEED_VALUE;
		ServoMotorInfo[motorIndex].TargetSpeed = SERVO_MOTOR_MAX_SPEED_VALUE;
		ServoMotorInfo[motorIndex].AccTime = 0;
		ServoMotorInfo[motorIndex].DecTime = 0;
		ServoMotorInfo[motorIndex].Status =	MOTOR_STATUS_OFF;

		ServoMotorInfo[motorIndex].LastCheckEncoderTime	= 0;
		ServoMotorInfo[motorIndex].OldAbsEncoderReadValue =	0;
		ServoMotorInfo[motorIndex].AbsEncoderVal = 0;
		ServoMotorInfo[motorIndex].LastCheckEncoderValue = 0;
		ServoMotorInfo[motorIndex].MinSpeedValue = 0;
		ServoMotorInfo[motorIndex].AutoHoldOff = TRUE;
	}

	if(MachineSetupData.MachineType == MACHINE_TYPE_SELMA_ADVANCED || MachineSetupData.MachineType == MACHINE_TYPE_LIMA_ADVANCED)
	{
		ServoMotorInfo[0].IO_PULSE_ADDR_H =	SERVO_MOTOR_0_PULSE_H;
		ServoMotorInfo[1].IO_PULSE_ADDR_H =	SERVO_MOTOR_1_PULSE_H;
		ServoMotorInfo[2].IO_PULSE_ADDR_H =	SERVO_MOTOR_2_PULSE_H;
		ServoMotorInfo[3].IO_PULSE_ADDR_H =	SERVO_MOTOR_3_PULSE_H;
		ServoMotorInfo[4].IO_PULSE_ADDR_H =	SERVO_MOTOR_4_PULSE_H;

		ServoMotorInfo[0].IO_PULSE_ADDR_L =	SERVO_MOTOR_0_PULSE_L;
		ServoMotorInfo[1].IO_PULSE_ADDR_L =	SERVO_MOTOR_1_PULSE_L;
		ServoMotorInfo[2].IO_PULSE_ADDR_L =	SERVO_MOTOR_2_PULSE_L;
		ServoMotorInfo[3].IO_PULSE_ADDR_L =	SERVO_MOTOR_3_PULSE_L;
		ServoMotorInfo[4].IO_PULSE_ADDR_L =	SERVO_MOTOR_4_PULSE_L;

		ServoMotorInfo[0].IO_CTRL_ADDR = SERVO_MOTOR_0_CONTROL;
		ServoMotorInfo[1].IO_CTRL_ADDR = SERVO_MOTOR_1_CONTROL;
		ServoMotorInfo[2].IO_CTRL_ADDR = SERVO_MOTOR_2_CONTROL;
		ServoMotorInfo[3].IO_CTRL_ADDR = SERVO_MOTOR_3_CONTROL;
		ServoMotorInfo[4].IO_CTRL_ADDR = SERVO_MOTOR_4_CONTROL;

		ServoMotorInfo[0].IOCtrlValue =	2;
		ServoMotorInfo[1].IOCtrlValue =	2;
		ServoMotorInfo[2].IOCtrlValue =	0;
		ServoMotorInfo[3].IOCtrlValue =	0;
		ServoMotorInfo[4].IOCtrlValue =	0;

		ServoMotorInfo[0].IOCtrlDirMask	= 0x0001;
		ServoMotorInfo[1].IOCtrlDirMask	= 0x0001;
		ServoMotorInfo[2].IOCtrlDirMask	= 0x0001;
		ServoMotorInfo[3].IOCtrlDirMask	= 0x0001;
		ServoMotorInfo[4].IOCtrlDirMask	= 0x0001;

		ServoMotorInfo[0].IOCtrlPulseOnMask	= 0x0004;
		ServoMotorInfo[1].IOCtrlPulseOnMask	= 0x0004;
		ServoMotorInfo[2].IOCtrlPulseOnMask	= 0x0004;
		ServoMotorInfo[3].IOCtrlPulseOnMask	= 0x0004;
		ServoMotorInfo[4].IOCtrlPulseOnMask	= 0x0004;

		ServoMotorInfo[0].ServoOnPortIndex = OP_SERVO_1_ON;
		ServoMotorInfo[1].ServoOnPortIndex = OP_SERVO_2_ON;
		ServoMotorInfo[2].ServoOnPortIndex = OP_SERVO_3_ON;
		ServoMotorInfo[3].ServoOnPortIndex = OP_SERVO_4_ON;
		ServoMotorInfo[4].ServoOnPortIndex = OP_SERVO_5_ON;

		ServoMotorInfo[0].EncoderIndex = SERVO_MOTOR_1_ENCODER_INDEX;
		ServoMotorInfo[1].EncoderIndex = SERVO_MOTOR_2_ENCODER_INDEX;
		ServoMotorInfo[2].EncoderIndex = SERVO_MOTOR_3_ENCODER_INDEX;
		ServoMotorInfo[3].EncoderIndex = SERVO_MOTOR_4_ENCODER_INDEX;
		ServoMotorInfo[4].EncoderIndex = SERVO_MOTOR_5_ENCODER_INDEX;	

		ServoOff(0);
		ServoOff(1);
		ServoOff(2);
		ServoOff(3);
		ServoOff(4);		
	}
	else
	{
		ServoMotorInfo[0].IO_PULSE_ADDR_H =	SERVO_MOTOR_0_PULSE_H;
		ServoMotorInfo[1].IO_PULSE_ADDR_H =	SERVO_MOTOR_1_PULSE_H;
		ServoMotorInfo[2].IO_PULSE_ADDR_H =	SERVO_MOTOR_3_PULSE_H;
		ServoMotorInfo[3].IO_PULSE_ADDR_H =	SERVO_MOTOR_4_PULSE_H;
		//ServoMotorInfo[4].IO_PULSE_ADDR_H =	SERVO_MOTOR_4_PULSE_H;

		ServoMotorInfo[0].IO_PULSE_ADDR_L =	SERVO_MOTOR_0_PULSE_L;
		ServoMotorInfo[1].IO_PULSE_ADDR_L =	SERVO_MOTOR_1_PULSE_L;
		ServoMotorInfo[2].IO_PULSE_ADDR_L =	SERVO_MOTOR_3_PULSE_L;
		ServoMotorInfo[3].IO_PULSE_ADDR_L =	SERVO_MOTOR_4_PULSE_L;
		//ServoMotorInfo[4].IO_PULSE_ADDR_L =	SERVO_MOTOR_4_PULSE_L;

		ServoMotorInfo[0].IO_CTRL_ADDR = SERVO_MOTOR_0_CONTROL;
		ServoMotorInfo[1].IO_CTRL_ADDR = SERVO_MOTOR_1_CONTROL;
		ServoMotorInfo[2].IO_CTRL_ADDR = SERVO_MOTOR_3_CONTROL;
		ServoMotorInfo[3].IO_CTRL_ADDR = SERVO_MOTOR_4_CONTROL;
		//ServoMotorInfo[4].IO_CTRL_ADDR = SERVO_MOTOR_4_CONTROL;

		ServoMotorInfo[0].IOCtrlValue =	2;
		ServoMotorInfo[1].IOCtrlValue =	2;
		ServoMotorInfo[2].IOCtrlValue =	0;
		ServoMotorInfo[3].IOCtrlValue =	0;
		//ServoMotorInfo[4].IOCtrlValue =	0;

		ServoMotorInfo[0].IOCtrlDirMask	= 0x0001;
		ServoMotorInfo[1].IOCtrlDirMask	= 0x0001;
		ServoMotorInfo[2].IOCtrlDirMask	= 0x0001;
		ServoMotorInfo[3].IOCtrlDirMask	= 0x0001;
		//ServoMotorInfo[4].IOCtrlDirMask	= 0x0001;

		ServoMotorInfo[0].IOCtrlPulseOnMask	= 0x0004;
		ServoMotorInfo[1].IOCtrlPulseOnMask	= 0x0004;
		ServoMotorInfo[2].IOCtrlPulseOnMask	= 0x0004;
		ServoMotorInfo[3].IOCtrlPulseOnMask	= 0x0004;
		//ServoMotorInfo[4].IOCtrlPulseOnMask	= 0x0004;

		ServoMotorInfo[0].ServoOnPortIndex = OP_SERVO_1_ON;
		ServoMotorInfo[1].ServoOnPortIndex = OP_SERVO_2_ON;
		ServoMotorInfo[2].ServoOnPortIndex = OP_SERVO_4_ON;
		ServoMotorInfo[3].ServoOnPortIndex = OP_SERVO_5_ON;
		//ServoMotorInfo[4].ServoOnPortIndex = OP_SERVO_5_ON;

		ServoMotorInfo[0].EncoderIndex = SERVO_MOTOR_1_ENCODER_INDEX;
		ServoMotorInfo[1].EncoderIndex = SERVO_MOTOR_2_ENCODER_INDEX;
		ServoMotorInfo[2].EncoderIndex = SERVO_MOTOR_3_ENCODER_INDEX;
		ServoMotorInfo[3].EncoderIndex = SERVO_MOTOR_4_ENCODER_INDEX;
		//ServoMotorInfo[4].EncoderIndex = SERVO_MOTOR_5_ENCODER_INDEX;	

		ServoOff(0);
		ServoOff(1);
		ServoOff(2);
		ServoOff(3);
		//ServoOff(4);	
	}



	// set first encoder value
}

void Servo_Motor_ChangeDir(int motorIndex, int dir)
{
	if (dir	!= 0)
	{
		ServoMotorInfo[motorIndex].IOCtrlValue |= ServoMotorInfo[motorIndex].IOCtrlDirMask;
		*(ServoMotorInfo[motorIndex].IO_CTRL_ADDR) = ServoMotorInfo[motorIndex].IOCtrlValue;
	}
	else
	{
		ServoMotorInfo[motorIndex].IOCtrlValue &= (~ServoMotorInfo[motorIndex].IOCtrlDirMask);
		*(ServoMotorInfo[motorIndex].IO_CTRL_ADDR) = ServoMotorInfo[motorIndex].IOCtrlValue;
	}
}


void Servo_Motor_Run_Param(int motorIndex, int accTime,	int	direction, unsigned	int	speed)
{
	if (speed == 0)	return;		// 0 means stop
	else if	(speed < ServoMotorInfo[motorIndex].MinSpeedValue)
	{
		speed =	ServoMotorInfo[motorIndex].MinSpeedValue;
	}
	else if	(speed >= SERVO_MOTOR_MAX_SPEED_VALUE)	// SERVO_MOTOR_MAX_SPEED_VALUE means stop too, but this	command	have to	run	something.
	{
		speed =	SERVO_MOTOR_MAX_SPEED_VALUE	- 1;
	}

	ServoMotorInfo[motorIndex].AccTime = accTime;
	ServoMotorInfo[motorIndex].Direction = direction;
	ServoMotorInfo[motorIndex].PrevSpeed = ServoMotorInfo[motorIndex].CurrentSpeed;
	ServoMotorInfo[motorIndex].TargetSpeed = speed;

	if (ServoMotorInfo[motorIndex].Status == MOTOR_STATUS_OFF)
	{
		ServoOn(motorIndex);
		ServoMotorInfo[motorIndex].ServoOnWaitingCount = SERVO_ON_WAITING_TIME - 1;
		ServoMotorInfo[motorIndex].Status =	MOTOR_STATUS_RUN_WAITING;
	}
	else if	(ServoMotorInfo[motorIndex].Status == MOTOR_STATUS_HOLD_WAITING)
	{
		ServoMotorInfo[motorIndex].Status =	MOTOR_STATUS_RUN_WAITING;
	}
	else if	(ServoMotorInfo[motorIndex].Status == MOTOR_STATUS_RUN ||
		ServoMotorInfo[motorIndex].Status == MOTOR_STATUS_ACC ||
		ServoMotorInfo[motorIndex].Status == MOTOR_STATUS_DEC ||
		ServoMotorInfo[motorIndex].Status == MOTOR_STATUS_HOLD_ON)
	{
		// start move
		Servo_Motor_ChangeDir(motorIndex, ServoMotorInfo[motorIndex].Direction);
		ServoMotorInfo[motorIndex].AccStartTime	= Board1MSTmr;
		ServoMotorInfo[motorIndex].Status =	MOTOR_STATUS_ACC;
	}
}

void Servo_Motor_Run(int motorIndex, int targetSpeed)
{
	Servo_Motor_Run_Param(motorIndex, ServoMotorSetupData[motorIndex].AccTime, ServoMotorSetupData[motorIndex].Dir,	targetSpeed);
}

void Servo_Motor_Stop_Param(int	motorIndex,	int	decTime)
{
	if (ServoMotorInfo[motorIndex].Status == MOTOR_STATUS_RUN ||
		ServoMotorInfo[motorIndex].Status == MOTOR_STATUS_ACC ||
		ServoMotorInfo[motorIndex].Status == MOTOR_STATUS_DEC)
	{
		ServoMotorInfo[motorIndex].DecTime = decTime;
		ServoMotorInfo[motorIndex].PrevSpeed = ServoMotorInfo[motorIndex].CurrentSpeed;
		ServoMotorInfo[motorIndex].TargetSpeed = SERVO_MOTOR_MAX_SPEED_VALUE;
		ServoMotorInfo[motorIndex].DecStartTime	= Board1MSTmr;
		ServoMotorInfo[motorIndex].Status =	MOTOR_STATUS_DEC;
	}
	else if	(ServoMotorInfo[motorIndex].Status == MOTOR_STATUS_RUN_WAITING ||
		ServoMotorInfo[motorIndex].Status == MOTOR_STATUS_HOLD_ON)
	{
		ServoMotorInfo[motorIndex].DecTime = 0;
		ServoMotorInfo[motorIndex].PrevSpeed = SERVO_MOTOR_MAX_SPEED_VALUE;
		ServoMotorInfo[motorIndex].CurrentSpeed	= SERVO_MOTOR_MAX_SPEED_VALUE;
		ServoMotorInfo[motorIndex].TargetSpeed = SERVO_MOTOR_MAX_SPEED_VALUE;
		ServoMotorInfo[motorIndex].Status =	MOTOR_STATUS_HOLD_ON;
		if (ServoMotorInfo[motorIndex].AutoHoldOff)
		{
			Servo_Motor_ServoOff(motorIndex);
		}
	}
	else
	{
		ServoMotorInfo[motorIndex].DecTime = 0;
		ServoMotorInfo[motorIndex].Status =	MOTOR_STATUS_HOLD_ON;
		if (ServoMotorInfo[motorIndex].AutoHoldOff)
		{
			Servo_Motor_ServoOff(motorIndex);
		}
	}
}


void Servo_Motor_Stop(int motorIndex)
{
	Servo_Motor_Stop_Param(motorIndex, ServoMotorSetupData[motorIndex].DecTime);
}

void Servo_Motor_ServoOn(int motorIndex)
{
	if (ServoMotorInfo[motorIndex].Status == MOTOR_STATUS_OFF)
	{
		ServoOn(motorIndex);
		ServoMotorInfo[motorIndex].ServoOnWaitingCount = SERVO_ON_WAITING_TIME - 1;
		ServoMotorInfo[motorIndex].Status =	MOTOR_STATUS_HOLD_WAITING;
	}
}

void Servo_Motor_ServoOff(int motorIndex)
{
	if (ServoMotorInfo[motorIndex].Status == MOTOR_STATUS_HOLD_ON ||
		ServoMotorInfo[motorIndex].Status == MOTOR_STATUS_OFF_WAITING ||
		ServoMotorInfo[motorIndex].Status == MOTOR_STATUS_OFF)
	{
		ServoMotorInfo[motorIndex].ServoOffWaitingCount	= SERVO_OFF_WAITING_TIME - 1;
		ServoMotorInfo[motorIndex].Status =	MOTOR_STATUS_OFF_WAITING;
	}
}

void Servo_Motor_SpeedChange(int motorIndex, int speed)
{
	int	timerPrdVal;
	int	timerPrdDiv, timerPrd;
	int 	sucktionDiskGearRatio;
#if 1	
	if (speed >	0)
	{
		sucktionDiskGearRatio = MachineSetupData.SuctionDiskGearRatio;	

		if(motorIndex < 2)
		{
			timerPrdVal	= speed/sucktionDiskGearRatio;  
		}
		else
		{
			timerPrdVal	= speed/10;
		}

		
		if (timerPrdVal	> 0x1FFF)
		{
			if(timerPrdVal > 0x5FFF)//8192*3 이상인	경우 (37% 이하의 속도에	해당 )
			{
				timerPrdVal	= (timerPrdVal*100)/(117+(timerPrdVal >> 13));
				timerPrdDiv	= ((timerPrdVal	>> 13) + 1)	& 0xFF;
				timerPrd = timerPrdVal / (timerPrdDiv +	2);
			}
			else//8192*2 이상이거나	그렇지 않은	경우(38% ~ 116%의 속도에 해당)
			{
				timerPrdDiv	= (timerPrdVal > 0x3FFF)? ((timerPrdVal	>> 13) & 0xFF):(((timerPrdVal >> 13) + 1) &	0xFF);
				timerPrd = timerPrdVal / (timerPrdDiv +	2);
			}
		}
		else//8192 이하인 경우(117%	~ 150%에 해당)
		{
			timerPrdDiv	= 2;
			timerPrd = timerPrdVal / 4;		
		}
		*(ServoMotorInfo[motorIndex].IO_PULSE_ADDR_L) =	timerPrd;
		*(ServoMotorInfo[motorIndex].IO_PULSE_ADDR_H) =	timerPrdDiv;
		ServoMotorInfo[motorIndex].IOCtrlValue |= ServoMotorInfo[motorIndex].IOCtrlPulseOnMask;
		*(ServoMotorInfo[motorIndex].IO_CTRL_ADDR) = ServoMotorInfo[motorIndex].IOCtrlValue;
	}
	else
	{
		ServoMotorInfo[motorIndex].IOCtrlValue &= (~ServoMotorInfo[motorIndex].IOCtrlPulseOnMask);
		*(ServoMotorInfo[motorIndex].IO_CTRL_ADDR) = ServoMotorInfo[motorIndex].IOCtrlValue;
	}	
#else

	if(motorIndex==2)
	{
		Moter3SpeedChangeOn = 1;
		Moter3Speed = ServoMotorBaseSpeed[motorIndex]*100/speed+1;
	}
	else
	{
		if (speed >	0)
		{
			sucktionDiskGearRatio = MachineSetupData.SuctionDiskGearRatio;	

			if(motorIndex < 2)
			{
				timerPrdVal	= speed/sucktionDiskGearRatio;  
			}
			else
			{
				timerPrdVal	= speed/10;
			}

			
			if (timerPrdVal	> 0x1FFF)
			{
				if(timerPrdVal > 0x5FFF)//8192*3 이상인	경우 (37% 이하의 속도에	해당 )
				{
					timerPrdVal	= (timerPrdVal*100)/(117+(timerPrdVal >> 13));
					timerPrdDiv	= ((timerPrdVal	>> 13) + 1)	& 0xFF;
					timerPrd = timerPrdVal / (timerPrdDiv +	2);
				}
				else//8192*2 이상이거나	그렇지 않은	경우(38% ~ 116%의 속도에 해당)
				{
					timerPrdDiv	= (timerPrdVal > 0x3FFF)? ((timerPrdVal	>> 13) & 0xFF):(((timerPrdVal >> 13) + 1) &	0xFF);
					timerPrd = timerPrdVal / (timerPrdDiv +	2);
				}
			}
			else//8192 이하인 경우(117%	~ 150%에 해당)
			{
				timerPrdDiv	= 2;
				timerPrd = timerPrdVal / 4;		
			}
			*(ServoMotorInfo[motorIndex].IO_PULSE_ADDR_L) =	timerPrd;
			*(ServoMotorInfo[motorIndex].IO_PULSE_ADDR_H) =	timerPrdDiv;
			ServoMotorInfo[motorIndex].IOCtrlValue |= ServoMotorInfo[motorIndex].IOCtrlPulseOnMask;
			*(ServoMotorInfo[motorIndex].IO_CTRL_ADDR) = ServoMotorInfo[motorIndex].IOCtrlValue;
		}
		else
		{
			ServoMotorInfo[motorIndex].IOCtrlValue &= (~ServoMotorInfo[motorIndex].IOCtrlPulseOnMask);
			*(ServoMotorInfo[motorIndex].IO_CTRL_ADDR) = ServoMotorInfo[motorIndex].IOCtrlValue;
		}
	}
#endif
}
void MoterSpeedChange()
{
	int speed;

	if(MachineSetupData.MachineType == MACHINE_TYPE_SELMA_ADVANCED || MachineSetupData.MachineType == MACHINE_TYPE_LIMA_ADVANCED)
	{	
		if(Moter3SpeedChangeOn)
		{

			speed = 800/Moter3Speed;


			if(Timer10usCount%speed<(speed/4))
			{
				PORT_ChangeOutPort(71,1);
			}
			else
			{
				PORT_ChangeOutPort(71,0);
			}
			if(Moter3Speed==0)
			{
				Moter3SpeedChangeOn = 0;
			}
			Timer10usCount++;		
		}
	}
}
void Servo_Motor_Process_1ms(void)		// per 1ms
{
	int	motorIndex;
	short tempEncoderVal;
	int checkServoMotorCount = SERVO_MOTOR_COUNT;	
	
	if(MachineSetupData.MachineType == MACHINE_TYPE_SELMA_ADVANCED || MachineSetupData.MachineType == MACHINE_TYPE_LIMA_ADVANCED)
	{
		checkServoMotorCount = SERVO_MOTOR_COUNT;
	}
	else
	{
		checkServoMotorCount = SERVO_MOTOR_COUNT - 1;
	}
		
	// check motor rpm
	for	(motorIndex	= 0; motorIndex	< checkServoMotorCount; motorIndex++)
	{
		tempEncoderVal = EncoderValue[ServoMotorInfo[motorIndex].EncoderIndex] & 0x0FFF;
		if (((tempEncoderVal - ServoMotorInfo[motorIndex].TempEncoderVal) &	0x0FFF)	< 0x0800)
		{
			ServoMotorInfo[motorIndex].AbsEncoderVal +=	((tempEncoderVal - ServoMotorInfo[motorIndex].TempEncoderVal) &	0x0FFF);
		}
		else
		{
			ServoMotorInfo[motorIndex].AbsEncoderVal -=	((ServoMotorInfo[motorIndex].TempEncoderVal	- tempEncoderVal) &	0x0FFF);
		}
		ServoMotorInfo[motorIndex].TempEncoderVal =	tempEncoderVal;
		if (Board1MSTmr	- ServoMotorInfo[motorIndex].LastCheckEncoderTime >= 1000)
		{
			ServoMotorInfo[motorIndex].MidTermMotorRPM = (int)(ServoMotorInfo[motorIndex].AbsEncoderVal	- ServoMotorInfo[motorIndex].LastCheckEncoderValue);

			if(MachineSetupData.LasercomplexerInfo.Machine_type==MACHINE_TYPE_SELMA_ADVANCED ||MachineSetupData.LasercomplexerInfo.Machine_type==MACHINE_TYPE_LIMA_ADVANCED)
			{
				if(MachineSetupData.LasercomplexerInfo.Machine_type==MACHINE_TYPE_SELMA_ADVANCED ||MachineSetupData.LasercomplexerInfo.Machine_type==MACHINE_TYPE_LIMA_ADVANCED)
				{
					//전자 기어비가 다름.
					if(motorIndex==3 ||motorIndex==4 )
					{
						ServoMotorInfo[motorIndex].MidTermMotorRPM = ServoMotorInfo[motorIndex].MidTermMotorRPM;
					}
					else
					{
						ServoMotorInfo[motorIndex].MidTermMotorRPM = (int)(ServoMotorInfo[motorIndex].MidTermMotorRPM*0.97);
					}
				}		
			}
			else
			{
				ServoMotorInfo[motorIndex].MidTermMotorRPM = (int)(ServoMotorInfo[motorIndex].MidTermMotorRPM*0.97);				
			}
			
			if(MachineSetupData.LasercomplexerInfo.Machine_type==MACHINE_TYPE_LIMA_BASIC ||MachineSetupData.LasercomplexerInfo.Machine_type==MACHINE_TYPE_LIMA_ADVANCED)
			{
				if(motorIndex < 2)
				{
					ServoMotorInfo[motorIndex].MidTermMotorRPM = ServoMotorInfo[motorIndex].MidTermMotorRPM /10;
				}
			}
			ServoMotorInfo[motorIndex].LastCheckEncoderTime	= Board1MSTmr;
			ServoMotorInfo[motorIndex].LastCheckEncoderValue = ServoMotorInfo[motorIndex].AbsEncoderVal;
		}
	}

}

void Servo_Motor_Process_20ms(void)		// per 20ms
{
	int	motorIndex;
	int	elapsedTime;
	int checkServoMotorCount = SERVO_MOTOR_COUNT;	
	
	if(MachineSetupData.MachineType == MACHINE_TYPE_SELMA_ADVANCED || MachineSetupData.MachineType == MACHINE_TYPE_LIMA_ADVANCED)
	{
		checkServoMotorCount = SERVO_MOTOR_COUNT;
	}
	else
	{
		checkServoMotorCount = SERVO_MOTOR_COUNT - 1;
	}	

	for	(motorIndex	= 0; motorIndex	< checkServoMotorCount; motorIndex++)
	{
		if (ServoMotorInfo[motorIndex].Status == MOTOR_STATUS_ACC)
		{
			if (ServoMotorInfo[motorIndex].AccTime == 0)		// instant run
			{
				ServoMotorInfo[motorIndex].CurrentSpeed	= ServoMotorInfo[motorIndex].TargetSpeed;
				Servo_Motor_SpeedChange(motorIndex,	ServoMotorInfo[motorIndex].CurrentSpeed);
				ServoMotorInfo[motorIndex].Status =	MOTOR_STATUS_RUN;
			}
			else if	(Board1MSTmr - ServoMotorInfo[motorIndex].AccStartTime < ServoMotorInfo[motorIndex].AccTime)
			{
				elapsedTime	= Board1MSTmr -	ServoMotorInfo[motorIndex].AccStartTime;
				if ((ServoMotorInfo[motorIndex].PrevSpeed +	(ServoMotorInfo[motorIndex].TargetSpeed	- ServoMotorInfo[motorIndex].PrevSpeed)	* elapsedTime /	ServoMotorInfo[motorIndex].AccTime)	> 0)
				{
					ServoMotorInfo[motorIndex].CurrentSpeed	= ServoMotorInfo[motorIndex].PrevSpeed * (float)ServoMotorInfo[motorIndex].TargetSpeed /
																(ServoMotorInfo[motorIndex].TargetSpeed	+ (ServoMotorInfo[motorIndex].PrevSpeed	- ServoMotorInfo[motorIndex].TargetSpeed) *
																(float)elapsedTime / ServoMotorInfo[motorIndex].AccTime);
					Servo_Motor_SpeedChange(motorIndex,	ServoMotorInfo[motorIndex].CurrentSpeed);
				}
			}
			else	// acc time	elapsed
			{
				ServoMotorInfo[motorIndex].CurrentSpeed	= ServoMotorInfo[motorIndex].TargetSpeed;
				//sjm del Servo_Motor_SpeedChange(motorIndex, ServoMotorInfo[motorIndex].CurrentSpeed);
				ServoMotorInfo[motorIndex].Status =	MOTOR_STATUS_RUN;
			}
		}
		else if	(ServoMotorInfo[motorIndex].Status == MOTOR_STATUS_DEC)
		{
			if (ServoMotorInfo[motorIndex].DecTime == 0)		// instant stop
			{
				ServoMotorInfo[motorIndex].CurrentSpeed	= SERVO_MOTOR_MAX_SPEED_VALUE;
				Servo_Motor_SpeedChange(motorIndex,	0);
				ServoMotorInfo[motorIndex].Status =	MOTOR_STATUS_HOLD_ON;
				if (ServoMotorInfo[motorIndex].AutoHoldOff)
				{
					Servo_Motor_ServoOff(motorIndex);
				}
			}
			else if	(Board1MSTmr - ServoMotorInfo[motorIndex].DecStartTime < ServoMotorInfo[motorIndex].DecTime)
			{
				elapsedTime	= Board1MSTmr -	ServoMotorInfo[motorIndex].DecStartTime;
				if (SERVO_MOTOR_MAX_SPEED_VALUE	+ (ServoMotorInfo[motorIndex].PrevSpeed	- SERVO_MOTOR_MAX_SPEED_VALUE) * elapsedTime / ServoMotorInfo[motorIndex].DecTime >	0)
				{
					ServoMotorInfo[motorIndex].CurrentSpeed	= ServoMotorInfo[motorIndex].PrevSpeed * (float)SERVO_MOTOR_MAX_SPEED_VALUE	/(SERVO_MOTOR_MAX_SPEED_VALUE + 
																(ServoMotorInfo[motorIndex].PrevSpeed - SERVO_MOTOR_MAX_SPEED_VALUE) * (float)elapsedTime / ServoMotorInfo[motorIndex].DecTime);
					Servo_Motor_SpeedChange(motorIndex,	ServoMotorInfo[motorIndex].CurrentSpeed);
				}
			}
			else	// dec time	elapsed
			{
				ServoMotorInfo[motorIndex].CurrentSpeed	= SERVO_MOTOR_MAX_SPEED_VALUE;
				Servo_Motor_SpeedChange(motorIndex,	0);
				ServoMotorInfo[motorIndex].Status =	MOTOR_STATUS_HOLD_ON;
				if (ServoMotorInfo[motorIndex].AutoHoldOff)
				{
					Servo_Motor_ServoOff(motorIndex);
				}
			}
		}
		else if	(ServoMotorInfo[motorIndex].Status == MOTOR_STATUS_RUN_WAITING)
		{
			ServoMotorInfo[motorIndex].ServoOnWaitingCount -= 20;
			if (ServoMotorInfo[motorIndex].ServoOnWaitingCount < 0)
			{
				Servo_Motor_ChangeDir(motorIndex, ServoMotorInfo[motorIndex].Direction);
				ServoMotorInfo[motorIndex].AccStartTime	= Board1MSTmr;
				ServoMotorInfo[motorIndex].Status =	MOTOR_STATUS_ACC;
			}
		}
		else if	(ServoMotorInfo[motorIndex].Status == MOTOR_STATUS_HOLD_WAITING)
		{
			ServoMotorInfo[motorIndex].ServoOnWaitingCount -= 20;
			if (ServoMotorInfo[motorIndex].ServoOnWaitingCount < 0)
			{
				ServoMotorInfo[motorIndex].Status =	MOTOR_STATUS_HOLD_ON;
			}
		}
		else if	(ServoMotorInfo[motorIndex].Status == MOTOR_STATUS_OFF_WAITING)
		{
			ServoMotorInfo[motorIndex].ServoOffWaitingCount	-= 20;
			if (ServoMotorInfo[motorIndex].ServoOffWaitingCount	< 0)
			{
				ServoOff(motorIndex);
				ServoMotorInfo[motorIndex].Status =	MOTOR_STATUS_OFF;
			}
		}
	}
}

int	Servo_Motor_CheckRun(int motorIndex)
{
	return (ServoMotorInfo[motorIndex].Status == MOTOR_STATUS_RUN);
}

int	Servo_Motor_CheckStopped(int motorIndex)		// this	means stop action has finished
{
	return (ServoMotorInfo[motorIndex].Status == MOTOR_STATUS_OFF_WAITING ||
		ServoMotorInfo[motorIndex].Status == MOTOR_STATUS_HOLD_ON ||
		ServoMotorInfo[motorIndex].Status == MOTOR_STATUS_OFF);
}


void ApplyServoMotorMinSpeedValue(void)
{
	int	motorIndex;
	int checkServoMotorCount = SERVO_MOTOR_COUNT;	
	
	if(MachineSetupData.MachineType == MACHINE_TYPE_SELMA_ADVANCED || MachineSetupData.MachineType == MACHINE_TYPE_LIMA_ADVANCED)
	{
		checkServoMotorCount = SERVO_MOTOR_COUNT;
	}
	else
	{
		checkServoMotorCount = SERVO_MOTOR_COUNT - 1;
	}	
	
	for	(motorIndex	= 0; motorIndex	< checkServoMotorCount; motorIndex++)
	{
		ServoMotorInfo[motorIndex].MinSpeedValue = ServoMotorBaseSpeed[motorIndex] / 2;
	}
}

void ApplyServoMotorSystemSetup(TServoMotorData	*servoMotorSetupData)
{
	memcpy(ServoMotorSetupData,	servoMotorSetupData, MAX_SERVO_MOTOR_COUNT * sizeof(TServoMotorData));
}


