table.c

CNC.rar

static void
TableStepHandler(tAxisState *pAxis)
{
    //
    // See if the last step has been reached.
    //
    if(pAxis->ulSteps == 0)
    {
        //
        // The last step has been reached, so set the motor current to holding
        // current.
        //
        pAxis->pfnCurrent(CURRENT_HOLDING);

        //
        // Indicate that the motor is no longer moving.
        //
        pAxis->bMoving = false;

        //
        // Return without doing anything further.
        //
        return;
    }

    //
    // Set the virtual timer to expire after the next inter-step delay.
    //
    pAxis->sTimer.ulTimeOut += (pAxis->ulC + 128) >> 8;
    VirtualTimerAdd(&(pAxis->sTimer));

    //
    // Step this motor.
    //
    pAxis->pfnStep(pAxis->lDir);

    //
    // Adjust the position by the step just taken.
    //
    pAxis->lPos += pAxis->lDir;

    //
    // See if the negative acceleration phase of the move has been reached.
    //
    if(pAxis->ulSteps <= pAxis->ulStopStep)
    {
        //
        // See if this is the first step of the negative acceleration phase of
        // the move.
        //
        if(pAxis->ulSteps == pAxis->ulStopStep)
        {
            //
            // Recompute the denominator to produce the desired negative
            // acceleration.
            //
            pAxis->lDenom = (pAxis->ulSteps * 4) - 1;
        }

        //
        // See if this is the last step of the move.
        //
        if(pAxis->lDenom == 5)
        {
            //
            // Treat the last step special, dividing the inter-step delay by
            // 0.4056 to account for the inaccuracy on the last step.
            //
            pAxis->ulC = MulDiv(pAxis->ulC, 10000, 4056);
        }
        else
        {
            //
            // Compute the new inter-step delay based on the previous one.
            //
            pAxis->ulC += (2 * pAxis->ulC) / pAxis->lDenom;
        }

        //
        // Decrease the denominator in preparation for the next step.
        //
        pAxis->lDenom -= 4;
    }
    else
    {
        //
        // See if the inter-step delay has reached the minimum (i.e. the motor
        // is running at the desired speed).
        //
        if(pAxis->ulC != pAxis->ulCMin)
        {
            //
            // Compute the new inter-step delay based on the previous one.  See
            // if this is the first step of the move.
            //
            if(pAxis->lDenom == 5)
            {
                //
                // Treat the first step special, multiplying the inter-step
                // delay by 0.4056 to account for the inaccuracy on the first
                // step.
                //
                pAxis->ulC = MulDiv(pAxis->ulC, 4056, 10000);
            }
            else
            {
                //
                // Compute the new inter-step delay based on the previous one.
                //
                pAxis->ulC -= (2 * pAxis->ulC) / pAxis->lDenom;
            }

            //
            // Increase the denominator in preparation for the next step.
            //
            pAxis->lDenom += 4;

            //
            // See if the inter-step delay is less than the minimum inter-step
            // delay.
            //
            if(pAxis->ulC < pAxis->ulCMin)
            {
                //
                // Limit the inter-step delay to the minimum so that the motor
                // does not spin faster than the desired speed.
                //
                pAxis->ulC = pAxis->ulCMin;
            }
        }
    }

    //
    // Decrement the count of steps to move take.
    //
    pAxis->ulSteps--;
}

//*****************************************************************************
//
//! The step handler for the X axis.
//!
//! This function is called when the timer for the X axis expires, causing a
//! step to be taken along the X axis.  Additionally, when the X axis is the
//! major axis and the Bresenham line drawing algorithm is being used, the Y
//! axis will also be optionally stepped when appropriate.
//!
//! \return None.
//
//*****************************************************************************
static void
TableXHandler(void)
{
    //
    // Step the X axis.
    //
    TableStepHandler(&g_sXAxis);

    //
    // See if the Bresenham line drawing algorithm is being used.
    //
    if(g_lTableDeltaX)
    {
        //
        // Increment the error term by the Y delta.
        //
        g_lTableError += g_lTableDeltaY;

        //
        // See if the error term has exceeded the X delta.
        //
        if((g_lTableError * 2) >= g_lTableDeltaX)
        {
            //
            // Step the Y axis.
            //
            g_sYAxis.pfnStep(g_sYAxis.lDir);

            //
            // Increment the Y axis position by the step just taken.
            //
            g_sYAxis.lPos += g_sYAxis.lDir;

            //
            // Decrement the error term by the X delta.
            //
            g_lTableError -= g_lTableDeltaX;
        }
    }
}

//*****************************************************************************
//
//! The step handler for the Y axis.
//!
//! This function is called when the timer for the Y axis expires, causing a
//! step to be taken along the Y axis.  Additionally, when the Y axis is the
//! major axis and the Bresenham line drawing algorithm is being used, the X
//! axis will also be optionally stepped when appropriate.
//!
//! \return None.
//
//*****************************************************************************
static void
TableYHandler(void)
{
    //
    // Step the Y axis.
    //
    TableStepHandler(&g_sYAxis);

    //
    // See if the Bresenham line drawing algorithm is being used.
    //
    if(g_lTableDeltaY)
    {
        //
        // Increment the error term by the X delta.
        //
        g_lTableError += g_lTableDeltaX;

        //
        // See if the error term has exceeded the Y delta.
        //
        if((g_lTableError * 2) >= g_lTableDeltaY)
        {
            //
            // Step the X axis.
            //
            g_sXAxis.pfnStep(g_sXAxis.lDir);

            //
            // Increment the X axis position by the step just taken.
            //
            g_sXAxis.lPos += g_sXAxis.lDir;

            //
            // Decrement the error term by the Y delta.
            //
            g_lTableError -= g_lTableDeltaY;
        }
    }
}

//*****************************************************************************
//
//! The step handler for the Z axis.
//!
//! This function is called when the timer for the Z axis expires, causing a
//! step to be taken along the Z axis.
//!
//! \return None.
//
//*****************************************************************************
static void
TableZHandler(void)
{
    //
    // Step the Z axis.
    //
    TableStepHandler(&g_sZAxis);
}

//*****************************************************************************
//
//! Prepares an axis to move to a new position.
//!
//! \param pAxis is a pointer to the state structure for the axis to be moved.
//! \param lNewPos is the new position for the given axis.
//! \param ulSpeed is the maximum speed, in steps per second, to move the axis
//! while making the move.
//! \param ulAccel is the maximum rate, in steps per second^2, to accelerate
//! the motor while making the move.  Double this value is used for the
//! negative acceleration at the end of the move.
//!
//! This function performs all the setup required to move an axis from its
//! present position to the specified position.  Once the setup is complete, it
//! will start the virtual timer that begins the move.
//!
//! The move consists of three phases; an acceleration phase at the beginning,
//! a constant speed phase in the middle, and a negative acceleration phase at
//! the end.  If the move is too short to accelerate up to top speed, then
//! there will be no constant speed phase in the middle; just an acceleration
//! phase and negative acceleration phase.
//!
//! \return None.
//
//*****************************************************************************
static void
TableMove(tAxisState *pAxis, long lNewPos, unsigned long ulSpeed,
          unsigned long ulAccel)
{
    unsigned long ulUp, ulDown;

    //
    // Determine the direction of the move.
    //
    if(lNewPos >= pAxis->lPos)
    {
        //
        // The move is in the positive direction.
        //
        pAxis->lDir = 1;

        //
        // Get the number of steps in the move.
        //
        pAxis->ulSteps = lNewPos - pAxis->lPos;
    }
    else
    {
        //
        // The move is in the negative direction.
        //
        pAxis->lDir = -1;

        //
        // Get the number of steps in the move.
        //
        pAxis->ulSteps = pAxis->lPos - lNewPos;
    }

    //
    // Get the number of steps required to accelerate up to the desired speed,
    // and to negatively accelerate down from the desired speed.
    //
    ulUp = GET_NUM_STEPS(ulSpeed, ulAccel);
    ulDown = GET_NUM_STEPS(ulSpeed, ulAccel * 2);

    //
    // See if there are enough steps in the move to have a constant speed
    // phase.
    //
    if(pAxis->ulSteps < (ulUp + ulDown))
    {
        //
        // There are not enough steps for a constant speed phase, so set the
        // transition point between acceleration and negative acceleration to
        // the two thirds point of the move (since accelerating at twice the
        // rate requires half the steps).
        //
        pAxis->ulStartStep = pAxis->ulSteps / 3;
        pAxis->ulStopStep = pAxis->ulSteps / 3;
    }
    else
    {
        //
        // Set the first step of the constant speed phase of the move.  This is
        // used by an early stop to determine how many steps would be required
        // to slow down to a stop.
        //
        pAxis->ulStartStep = pAxis->ulSteps - ulUp;

        //
        // Set the first step of the negative acceleration phase of the move.
        //
        pAxis->ulStopStep = ulDown;
    }

    //
    // Get the starting inter-step delay.
    //
    pAxis->ulC = GET_C0(ulAccel);

    //
    // Get the minimum inter-step delay.
    //
    pAxis->ulCMin = GET_CMIN(ulSpeed);

    //
    // Set the starting denominator for the incremental inter-step delay
    // computation.
    //
    pAxis->lDenom = 5;

    //
    // Set the current of this motor to running current.
    //
    pAxis->pfnCurrent(CURRENT_RUNNING);

    //
    // Start the virtual timer for this axis.
    //
    pAxis->sTimer.ulTimeOut = VirtualTimeGet() + ((pAxis->ulC + 128) >> 8);
    VirtualTimerAdd(&(pAxis->sTimer));

    //
    // Indicate that this axis is moving.
    //
    pAxis->bMoving = true;
}

//*****************************************************************************
//
//! Moves the table to a new position.
//!
//! \param lNewX is the new position for the X axis.
//! \param lNewY is the new position for the Y axis.
//! \param lNewZ is the new position for the Z axis.
//! \param ulSpeed is the maximum speed to move the tool along the line.
//! \param ulAccel is the maximum acceleration of the tool along the line.
//!
//! This function starts a move of the table along a straight line to a new
//! position.  Arbitrary moves can be made in the X-Y plane, or moves can be
//! made along the Z axis, but moves can not be made in the X-Z or Y-Z axes or
//! in 3-space.
//!
//! \return None.
//
//*****************************************************************************
void
TableMoveLine(long lNewX, long lNewY, long lNewZ, unsigned long ulSpeed,