Ejemplo n.º 1
0
inline static int moveOV8825AF(unsigned long a_u4Position)
{
	int ret = 0;

	if ((a_u4Position > g_u4OV8825AF_MACRO) || (a_u4Position < g_u4OV8825AF_INF)) {
		OV8825AFDB("[OV8825AF] out of range\n");
		return -EINVAL;
	}

	if (g_s4OV8825AF_Opened == 1) {
		unsigned short InitPos;
		ret = s4OV8825AF_ReadReg(&InitPos);


		if (ret == 0) {
			OV8825AFDB("[OV8825AF] Init Pos %6d\n", InitPos);

			spin_lock(&g_OV8825AF_SpinLock);
			g_u4CurrPosition = (unsigned long)InitPos;
			spin_unlock(&g_OV8825AF_SpinLock);
		} else {
			spin_lock(&g_OV8825AF_SpinLock);
			g_u4CurrPosition = 0;
			spin_unlock(&g_OV8825AF_SpinLock);
		}
		spin_lock(&g_OV8825AF_SpinLock);
		g_s4OV8825AF_Opened = 2;
		spin_unlock(&g_OV8825AF_SpinLock);
	}

	if (g_u4CurrPosition < a_u4Position) {
		spin_lock(&g_OV8825AF_SpinLock);
		g_i4Dir = 1;
		spin_unlock(&g_OV8825AF_SpinLock);
	} else if (g_u4CurrPosition > a_u4Position) {
		spin_lock(&g_OV8825AF_SpinLock);
		g_i4Dir = -1;
		spin_unlock(&g_OV8825AF_SpinLock);
	} else {
		return 0;
	}

	spin_lock(&g_OV8825AF_SpinLock);
	g_u4TargetPosition = a_u4Position;
	spin_unlock(&g_OV8825AF_SpinLock);

	/* OV8825AFDB("[OV8825AF] move [curr] %d [target] %d\n", g_u4CurrPosition, g_u4TargetPosition); */

	spin_lock(&g_OV8825AF_SpinLock);
	g_sr = 3;
	g_i4MotorStatus = 0;
	spin_unlock(&g_OV8825AF_SpinLock);

	if (s4OV8825AF_WriteReg((unsigned short)g_u4TargetPosition) == 0) {
		spin_lock(&g_OV8825AF_SpinLock);
		g_u4CurrPosition = (unsigned long)g_u4TargetPosition;
		spin_unlock(&g_OV8825AF_SpinLock);
	} else {
		OV8825AFDB("[OV8825AF] set I2C failed when moving the motor\n");
		spin_lock(&g_OV8825AF_SpinLock);
		g_i4MotorStatus = -1;
		spin_unlock(&g_OV8825AF_SpinLock);
	}

	return 0;
}
Ejemplo n.º 2
0
inline static int moveOV8825AF(unsigned long a_u4Position)
{
    if((a_u4Position > g_u4OV8825AF_MACRO) || (a_u4Position < g_u4OV8825AF_INF))
    {
        OV8825AFDB("[OV8825AF] out of range \n");
        return -EINVAL;
    }

	if (g_s4OV8825AF_Opened == 1)
	{
		unsigned short InitPos;
	
		if(s4OV8825AF_ReadReg(&InitPos) == 0)
		{
			OV8825AFDB("[OV8825AF] Init Pos %6d \n", InitPos);
		
			g_u4CurrPosition = (unsigned long)InitPos;
		}
		else
		{
			g_u4CurrPosition = 0;
		}
		
		g_s4OV8825AF_Opened = 2;
	}

	if      (g_u4CurrPosition < a_u4Position)	{g_i4Dir = 1;}
	else if (g_u4CurrPosition > a_u4Position)	{g_i4Dir = -1;}
	else										{return 0;}

	if (1)
	{
		g_i4Position = (long)g_u4CurrPosition;
		g_u4TargetPosition = a_u4Position;

		if (g_i4Dir == 1)
		{
			//if ((g_u4TargetPosition - g_u4CurrPosition)<60)
			{		
				g_i4MotorStatus = 0;
				if(s4OV8825AF_WriteReg((unsigned short)g_u4TargetPosition) == 0)
				{
					g_u4CurrPosition = (unsigned long)g_u4TargetPosition;
				}
				else
				{
					OV8825AFDB("[OV8825AF] set I2C failed when moving the motor \n");
					g_i4MotorStatus = -1;
				}
			}
			//else
			//{
			//	g_i4MotorStatus = 1;
			//}
		}
		else if (g_i4Dir == -1)
		{
			//if ((g_u4CurrPosition - g_u4TargetPosition)<60)
			{
				g_i4MotorStatus = 0;		
				if(s4OV8825AF_WriteReg((unsigned short)g_u4TargetPosition) == 0)
				{
					g_u4CurrPosition = (unsigned long)g_u4TargetPosition;
				}
				else
				{
					OV8825AFDB("[OV8825AF] set I2C failed when moving the motor \n");
					g_i4MotorStatus = -1;
				}
			}
			//else
			//{
			//	g_i4MotorStatus = 1;		
			//}
		}
	}
	else
	{
	g_i4Position = (long)g_u4CurrPosition;
	g_u4TargetPosition = a_u4Position;
	g_i4MotorStatus = 1;
	}

    return 0;
}
Ejemplo n.º 3
0
inline static int moveOV8825AF(unsigned long a_u4Position)
{
    int ret = 0;

    OV8825AFDB("a_u4Position = %d \n", a_u4Position);
    if((a_u4Position > g_u4OV8825AF_MACRO) || (a_u4Position < g_u4OV8825AF_INF))
    {
        OV8825AFDB("[OV8825AF] out of range \n");
        return -EINVAL;
    }

    if (g_s4OV8825AF_Opened == 1)
    {
        unsigned short InitPos;
        ret = s4OV8825AF_ReadReg(&InitPos);
	    
        spin_lock(&g_OV8825AF_SpinLock);
        if(ret == 0)
        {
            OV8825AFDB("[OV8825AF] Init Pos %6d \n", InitPos);
            g_u4CurrPosition = (unsigned long)InitPos;
        }
        else
        {		
            g_u4CurrPosition = 0;
        }
        g_s4OV8825AF_Opened = 2;
        spin_unlock(&g_OV8825AF_SpinLock);
    }

    if (g_u4CurrPosition < a_u4Position)
    {
        spin_lock(&g_OV8825AF_SpinLock);	
        g_i4Dir = 1;
        spin_unlock(&g_OV8825AF_SpinLock);	
    }
    else if (g_u4CurrPosition > a_u4Position)
    {
        spin_lock(&g_OV8825AF_SpinLock);	
        g_i4Dir = -1;
        spin_unlock(&g_OV8825AF_SpinLock);			
    }
    else										{return 0;}
    	
    	
    spin_lock(&g_OV8825AF_SpinLock);
    //g_i4Position = (long)g_u4CurrPosition;
    g_u4TargetPosition = a_u4Position;       
    g_sr = 3;
    g_i4MotorStatus = 0;
    spin_unlock(&g_OV8825AF_SpinLock);    

            
    if(abs(g_u4CurrPosition - g_u4TargetPosition) > MAX_MOVE_STEP)
    {
        OV8825AFDB("[OV8825AF] in small stepLeave breakdown \n");
        breakDownSteps(MAX_MOVE_STEP, MOVE_INTERVAL, g_u4CurrPosition, g_u4TargetPosition);
    } 
    else
    {   
        if(s4OV8825AF_WriteReg((unsigned short)g_u4TargetPosition) == 0)
        {
            spin_lock(&g_OV8825AF_SpinLock);      
            g_u4CurrPosition = (unsigned long)g_u4TargetPosition;
            spin_unlock(&g_OV8825AF_SpinLock);                
        }
        else
        {
            OV8825AFDB("[OV8825AF] set I2C failed when moving the motor \n");         
            spin_lock(&g_OV8825AF_SpinLock);
            g_i4MotorStatus = -1;
            spin_unlock(&g_OV8825AF_SpinLock);             
        }
    }

    return 0;
}