table.c

CNC.rar

//! Multiplies a number by a fraction specified as a numerator and denominator.
//!
//! \param ulValue is the value to be multiplied.
//! \param ulNum is the numerator of the fraction.
//! \param ulDenom is the denominator of the fraction.
//!
//! This function take an integer and multiplies it by a fraction specified by
//! a numerator and denominator.  Assuming infinite precision of the operands,
//! this would be "(ulValue * ulNum) / ulDenom", though with the fixed
//! precision of the processor the multiply would overflow if performed as is.
//!
//! \return The result of the multiplication.
//
//*****************************************************************************
static unsigned long
MulDiv(unsigned long ulValue, unsigned long ulNum, unsigned long ulDenom)
{
    //
    // Perform the multiplication in pieces to maintain the full accuracy and
    // return the result.
    //
    return(((((ulValue / 65536) * ulNum) / ulDenom) * 65536) +
           (((((ulValue / 65536) * ulNum) % ulDenom) * 65536) / ulDenom) +
           (((ulValue % 65536) * ulNum) / ulDenom));
}

//*****************************************************************************
//
//! Divides two numbers, returning the result in 24.8 fixed-point notation.
//!
//! \param ulNum is the numerator for the division.
//! \param ulDenom is the denominator for the division.
//!
//! This function takes two integers and divides them, returning the results in
//! 24.8 fixed-point notation (with full accuracy).  Assuming infinite
//! precision of the operands, this would be "(256 * ulNum) / ulDenom", though
//! with the fixed precision of the processor the multiply would overflow if
//! performed as is.
//!
//! \return The result of the division.
//
//*****************************************************************************
static unsigned long
LongDiv256(unsigned long ulNum, unsigned long ulDenom)
{
    //
    // Perform the division in pieces to maintain the full accuracy and return
    // the 24.8 fixed-point result.
    //
    return(((ulNum / ulDenom) * 256) + (((ulNum % ulDenom) * 256) / ulDenom));
}

//*****************************************************************************
//
//! The timer handler for the table homing operation.
//!
//! This is the timer handler that is called after each step delay during the
//! table homing operation.  There are four phases to the homing operation:
//!
//! - In the first phase, the tool is rapidly raised until the home limit
//!   switch engages.
//!
//! - In the second phase, the table is rapidly jogged until the home limit
//!   switches engage.
//!
//! - In the third phase, the tool is slowly lowered until the home limit
//!   switch disengages.
//!
//! - In the final phase, the table is slowly moved away from the home position
//!   until the home limit switches disengage.
//!
//! Once complete, this becomes the home position.  Since there may be some
//! positional overshoot when rapidly moving toward the home position (since
//! it might not be possible to immediately stop the table once the limit
//! switch engages), the slow move until the limit switches disengage is
//! performed at a speed that can be reliably stopped immediately.  This
//! provides a more accurate and repeatable home position (subject to the
//! mechanical variablility of the limit switches themselves).
//!
//! \return None.
//
//*****************************************************************************
static void
TableHomeHandler(void)
{
    unsigned long ulSwitches;
    tBoolean bContinue;

    //
    // Set the continuation indicator to false.
    //
    bContinue = false;

    //
    // Read the limit switches.
    //
    ulSwitches = SwitchesLimitRead();

    //
    // See if the first or second phase of the homing operation is in progress.
    //
    if(g_ulHomeState == STATE_HOMING1)
    {
        //
        // See if the Z home limit switch is engaged.
        //
        if((ulSwitches & SWITCH_LIMIT_Z_MINUS) == 0)
        {
            //
            // Step the Z axis in the negative direction.
            //
            g_sZAxis.pfnStep(-1);
        }
        else
        {
            //
            // Move the phase two of the homing operation.
            //
            g_ulHomeState = STATE_HOMING2;

            //
            // Set the inter-step delay such that the second phase of the
            // homing operation happens slowly.
            //
            g_sXAxis.ulC = GET_C0(30000);
            g_sXAxis.lDenom = 5;
        }

        //
        // Indicate that the homing operation should continue.
        //
        bContinue = true;
    }

    else if(g_ulHomeState == STATE_HOMING2)
    {
        //
        // See if the X home limit switch is engaged.
        //
        if((ulSwitches & SWITCH_LIMIT_X_MINUS) == 0)
        {
            //
            // Step the X axis in the negative direction.
            //
            g_sXAxis.pfnStep(-1);

            //
            // Phase one of the homing operation should continue.
            //
            bContinue = true;
        }

        //
        // See if the Y home limit swtich is engaged.
        //
        if((ulSwitches & SWITCH_LIMIT_Y_MINUS) == 0)
        {
            //
            // Step the Y axis in the negative direction.
            //
            g_sYAxis.pfnStep(-1);

            //
            // Phase one of the homing operation should continue.
            //
            bContinue = true;
        }

        //
        // See if phase two of the homing operation has completed.
        //
        if(!bContinue)
        {
            //
            // Move the phase three of the homing operation.
            //
            g_ulHomeState = STATE_HOMING3;

            //
            // Set the inter-step delay such that the second phase of the
            // homing operation happens slowly.
            //
            g_sXAxis.ulC = GET_C0(30000);

            //
            // Indicate that the homing operation should continue.
            //
            bContinue = true;
        }
    }

    else if(g_ulHomeState == STATE_HOMING3)
    {
        //
        // See if the X home limit switch is still engaged.
        //
        if(ulSwitches & SWITCH_LIMIT_X_MINUS)
        {
            //
            // Step the X axis in the positive direction.
            //
            g_sXAxis.pfnStep(1);

            //
            // Phase two of the homing operation should continue.
            //
            bContinue = true;
        }

        //
        // See if the Y home limit switch is still engaged.
        //
        if(ulSwitches & SWITCH_LIMIT_Y_MINUS)
        {
            //
            // Step the Y axis in the positive direction.
            //
            g_sYAxis.pfnStep(1);

            //
            // Phase two of the homing operation should continue.
            //
            bContinue = true;
        }

        //
        // See if phase three of the homing operation has completed.
        //
        if(!bContinue)
        {
            //
            // Move the phase four of the homing operation.
            //
            g_ulHomeState = STATE_HOMING4;

            //
            // Set the inter-step delay such that the fourth phase of the
            // homing operation happens slowly.
            //
            g_sXAxis.ulC = GET_C0(30000);

            //
            // Indicate that the homing operation should continue.
            //
            bContinue = true;
        }
    }

    else
    {
        //
        // See if the Z home limit switch is still engaged.
        //
        if(ulSwitches & SWITCH_LIMIT_Z_MINUS)
        {
            //
            // Step the Z axis in the positive direction.
            //
            g_sZAxis.pfnStep(1);

            //
            // Phase two of the homing operation should continue.
            //
            bContinue = true;
        }
    }

    //
    // See if the homing operation should continue.
    //
    if(bContinue)
    {
        //
        // See if the first or second phase of the homing operation is process.
        //
        if((g_ulHomeState == STATE_HOMING1) ||
           (g_ulHomeState == STATE_HOMING2))
        {
            //
            // For the first phase of the homing operation, restart the timer
            // and compute a new value for the inter-step delay.
            //
            g_sHomeTimer.ulTimeOut += (g_sXAxis.ulC + 128) >> 8;
            VirtualTimerAdd(&g_sHomeTimer);

            //
            // See if the inter-step delay has reached the minimum.
            //
            if(g_sXAxis.ulC != g_sXAxis.ulCMin)
            {
                //
                // Compute the new value of the inter-step delay.  Treat the
                // first step specially to account for the relatively large
                // error that would otherwise occur.
                //
                if(g_sXAxis.lDenom == 5)
                {
                    g_sXAxis.ulC = MulDiv(g_sXAxis.ulC, 4056, 10000);
                }
                else
                {
                    g_sXAxis.ulC -= (2 * g_sXAxis.ulC) / g_sXAxis.lDenom;
                }

                //
                // Limit the inter-step delay to the minimum value.
                //
                if(g_sXAxis.ulC < g_sXAxis.ulCMin)
                {
                    g_sXAxis.ulC = g_sXAxis.ulCMin;
                }

                //
                // Increment the denominator so that the next inter-step delay
                // is computed correctly.
                //
                g_sXAxis.lDenom += 4;
            }
        }
        else
        {
            //
            // For the second phase of the homing operation, keep the same
            // inter-step delay so that the table is moved slowly.
            //
            g_sHomeTimer.ulTimeOut += (g_sXAxis.ulC + 128) >> 8;
            VirtualTimerAdd(&g_sHomeTimer);
        }
    }
    else
    {
        //
        // The homing operation is complete.
        //
        g_ulHomeState = STATE_HOMED;
    }
}

//*****************************************************************************
//
//! Finds the table home position.
//!
//! This function moves the table until it is in the home position.  After
//! finding the home position, any required table move can be made in a
//! repeatable manner.
//!
//! \return None.
//
//*****************************************************************************
void
TableHome(void)
{
    //
    // Set the current of each motor to running.
    //
    g_sXAxis.pfnCurrent(CURRENT_RUNNING);
    g_sYAxis.pfnCurrent(CURRENT_RUNNING);
    g_sZAxis.pfnCurrent(CURRENT_RUNNING);

    //
    // Set the state to the first phase of the homing operation.
    //
    g_ulHomeState = STATE_HOMING1;

    //
    // Setup the acceleration for the homing operation.  Arbitrarily use the X
    // axis to store the information.
    //
    g_sXAxis.ulC = GET_C0(30000);
    g_sXAxis.ulCMin = GET_CMIN(1200);
    g_sXAxis.lDenom = 5;

    //
    // Start the homing operation timer.
    //
    g_sHomeTimer.ulTimeOut = VirtualTimeGet() + ((g_sXAxis.ulC + 128) >> 8);
    VirtualTimerAdd(&g_sHomeTimer);

    //
    // Wait until the homing operation is complete.
    //
    while(g_ulHomeState != STATE_HOMED)
    {
    }

    //
    // Reset the position of each axis.
    //
    g_sXAxis.lPos = 0;
    g_sYAxis.lPos = 0;
    g_sZAxis.lPos = 0;

    //
    // Set the current of each motor to holding.
    //
    g_sXAxis.pfnCurrent(CURRENT_HOLDING);
    g_sYAxis.pfnCurrent(CURRENT_HOLDING);
    g_sZAxis.pfnCurrent(CURRENT_HOLDING);
}

//*****************************************************************************
//
//! The step handler for an axis.
//!
//! \param pAxis is a pointer to the axis to be stepped.
//!
//! This is a generic axis step handler; it will perform a step operation on
//! the given axis.
//!
//! \return None.
//
//*****************************************************************************