table.c

CNC.rar

              unsigned long ulAccel)
{
    long lDX, lDY, lRatio, lDiagonal;

    //
    // Return without doing anything if the table is already at the given
    // position.
    //
    if((lNewX == g_sXAxis.lPos) && (lNewY == g_sYAxis.lPos) &&
       (lNewZ == g_sZAxis.lPos))
    {
        return;
    }

    //
    // Return without doing anything if an invalid move was requested (i.e. not
    // in the X-Y plane or along the Z axis).
    //
    if(((lNewX != g_sXAxis.lPos) || (lNewY != g_sYAxis.lPos)) &&
       (lNewZ != g_sZAxis.lPos))
    {
        return;
    }

    //
    // See if this is a move along the Z axis.
    //
    if(lNewZ != g_sZAxis.lPos)
    {
        //
        // Setup the Z axis move.
        //
        TableMove(&g_sZAxis, lNewZ, ulSpeed, ulAccel);

        //
        // The move has been started, so no further work needs to be done.
        //
        return;
    }

    //
    // Determine the number of steps along the X axis of the requested move.
    //
    lDX = ((lNewX > g_sXAxis.lPos) ? (lNewX - g_sXAxis.lPos) :
           (g_sXAxis.lPos - lNewX));

    //
    // Determine the number of steps along the Y axis of the requested move.
    //
    lDY = ((lNewY > g_sYAxis.lPos) ? (lNewY - g_sYAxis.lPos) :
           (g_sYAxis.lPos - lNewY));

    //
    // See if the move is along the X or Y axis (i.e. one of the axes has no
    // move).
    //
    if((lDX == 0) || (lDY == 0))
    {
        //
        // Clear the X and Y deltas, so that the Bresenham line drawing
        // algorithm is turned off.
        //
        g_lTableDeltaX = 0;
        g_lTableDeltaY = 0;

        //
        // See if this move is in the X or Y axis.
        //
        if(lDY == 0)
        {
            //
            // Setup the X axis move.
            //
            TableMove(&g_sXAxis, lNewX, ulSpeed, ulAccel);
        }
        else
        {
            //
            // Setup the Y axis move.
            //
            TableMove(&g_sYAxis, lNewY, ulSpeed, ulAccel);
        }

        //
        // The move has been started, so no further work needs to be done.
        //
        return;
    }

    //
    // Determine the ratio of the X and Y delta to get the slope of the line
    // (this is to cotan() of the angle).
    //
    lRatio = (lDX > lDY) ? (lDX / lDY) : (lDY / lDX);

    //
    // Get the length of the line.
    //
    lDiagonal = isqrt((lDX * lDX) + (lDY * lDY));

    //
    // If the line is within ~10 degrees of the X or Y axis (i.e. the ratio is
    // greater 5), then use then use the Bresenham line drawing algorithm to
    // make this move.
    //
    if(lRatio > 5)
    {
        //
        // Set the X delta to the difference along the X axis.
        //
        g_lTableDeltaX = lDX;

        //
        // Set the Y delta to the difference along the Y axis.
        //
        g_lTableDeltaY = lDY;

        //
        // The error starts at zero.
        //
        g_lTableError = 0;

        //
        // See if the X or Y axis is the major axis.
        //
        if(lDX > lDY)
        {
            //
            // The X axis is the major axis, so setup the X axis move.  Scale
            // the speed and acceleration by the length of the diagonal versus
            // the size of the move along the X axis to keep the speed and
            // acceleration of the tool along the diagonal from exceeding the
            // requested values.
            //
            TableMove(&g_sXAxis, lNewX, (ulSpeed * lDX) / lDiagonal,
                      (ulAccel * lDX) / lDiagonal);

            //
            // Set the direction of the move along the Y axis.
            //
            g_sYAxis.lDir = (lNewY >= g_sYAxis.lPos) ? 1 : -1;

            //
            // Set the current in the Y motor to running current.
            //
            g_sYAxis.pfnCurrent(CURRENT_RUNNING);
        }
        else
        {
            //
            // The Y axis is the major axis, so set the direction of the move
            // along the X axis.
            //
            g_sXAxis.lDir = (lNewX >= g_sXAxis.lPos) ? 1 : -1;

            //
            // Set the current in the X motor to running current.
            //
            g_sXAxis.pfnCurrent(CURRENT_RUNNING);

            //
            // Setup the Y axis move.  Scale the speed and acceleration by the
            // length of the diagonal versus the size of the move along the Y
            // axis to keep the speed and acceleration of the tool along the
            // diagonal from exceeding the requested values.
            //
            TableMove(&g_sYAxis, lNewY, (ulSpeed * lDY) / lDiagonal,
                      (ulAccel * lDY) / lDiagonal);
        }
    }
    else
    {
        //
        // Clear the X and Y deltas, so that the Bresenham line drawing
        // algorithm is turned off.
        //
        g_lTableDeltaX = 0;
        g_lTableDeltaY = 0;

        //
        // Setup the X axis move.  Scale the speed and acceleration by the
        // length of the diagonal versus the size of the move along the X axis
        // to keep the speed and acceleration of the tool along the diagonal
        // from exceeding the requested values.
        //
        TableMove(&g_sXAxis, lNewX, (ulSpeed * lDX) / lDiagonal,
                  (ulAccel * lDX) / lDiagonal);

        //
        // Setup the Y axis move.  Scale the speed and acceleration by the
        // length of the diagonal versus the size of the move along the Y axis
        // to keep the speed and acceleration of the tool along the diagonal
        // from exceeding the requested values.
        //
        TableMove(&g_sYAxis, lNewY, (ulSpeed * lDY) / lDiagonal,
                  (ulAccel * lDY) / lDiagonal);
    }
}

//*****************************************************************************
//
//! Stop the motion on the given axis.
//!
//! \param pAxis is the axis whose motion should be stopped.
//!
//! This function will stop the motion on a given axis.  If the axis is still
//! in the acceleration phase, it is moved to the negative acceleration phase
//! at the point appropriate to the current speed and allowed to negatively
//! accelerate to a stop.  If the axis is in the constant speed phase, it is
//! moved to the beginning of the negative acceleration phase.  If the phase is
//! in the negative acceleration phase, then it is left alone as it is already
//! stopping on its own.
//!
//! \return None.
//
//*****************************************************************************
static void
TableStopAxis(tAxisState *pAxis)
{
    //
    // See if the axis is still in the acceleration phase.
    //
    if(pAxis->ulSteps > pAxis->ulStartStep)
    {
        //
        // Compute the number of steps requires to negatively accelerate to a
        // stop from the current speed.
        //
        pAxis->ulSteps = ((pAxis->ulStopStep * 2) -
                          (pAxis->ulSteps - pAxis->ulStartStep)) / 2;

        //
        // Compute the new value for the denominator for computing the next
        // inter-step delay.
        //
        pAxis->lDenom = (pAxis->ulSteps * 4) - 1;
    }

    //
    // Otherwise, see if the axis is in the constant speed phase.
    //
    else if(pAxis->ulSteps > pAxis->ulStopStep)
    {
        //
        // Move the axis to the start of the negative acceleration phase,
        // causing it to stop.
        //
        pAxis->ulSteps = pAxis->ulStopStep;
    }
}

//*****************************************************************************
//
//! Stop motion of the table.
//!
//! This function will stop all motion on the table.  Any axis that is in
//! motion will be negatively accelerated to a stop at the rate that was
//! programmed for the motion.
//!
//! \return None.
//
//*****************************************************************************
void
TableStop(void)
{
    //
    // Stop all three axes.
    //
    TableStopAxis(&g_sXAxis);
    TableStopAxis(&g_sYAxis);
    TableStopAxis(&g_sZAxis);
}

//*****************************************************************************
//
//! Quickly stop motion of the table.
//!
//! This function will immediately stop all motion on the table and turn off
//! the driving current to all three axes.  Doing so will likely cause the
//! motors to continue spinning by an unpredictable amount, especially if they
//! are running at a high speed.  As a result, a homing operation is required
//! after calling this function.
//!
//! The intention of this function is to stop the motion on the table in an
//! emergency situation, where safety is more important than maintaining
//! control and positional knowledge of the motors.
//!
//! \return None.
//
//*****************************************************************************
void
TableEmergencyStop(void)
{
    //
    // Set the number of steps to be taken on all three axes to zero.
    //
    g_sXAxis.ulSteps = 0;
    g_sYAxis.ulSteps = 0;
    g_sZAxis.ulSteps = 0;

    //
    // Set the current on all three axes to holding current.
    //
    g_sXAxis.pfnCurrent(CURRENT_HOLDING);
    g_sYAxis.pfnCurrent(CURRENT_HOLDING);
    g_sZAxis.pfnCurrent(CURRENT_HOLDING);

    //
    // Indicate that the table will require a homing operation in order to
    // regain positional knowledge.
    //
    g_ulHomeState = STATE_NEEDS_HOMING;
}

//*****************************************************************************
//
//! Determines if the table is moving.
//!
//! This function determines if any of the axes on the table are moving.  Table
//! motion should be allowed to complete before calling TableMoveLine() to
//! initiate a new motion; otherwise, control of the motors will likely be lost
//! since the negative acceleration phase will be skipped or cut short.
//!
//! \return Returns a boolean that is \b true if any of the table axes are
//! still moving and \b false if not.
//
//*****************************************************************************
tBoolean
TableIsMoving(void)
{
    //
    // Determine if any of the table axes are moving.
    //
    return(g_sXAxis.bMoving || g_sYAxis.bMoving || g_sZAxis.bMoving);
}

//*****************************************************************************
//
//! Determines if the table requires a homing operation.
//!
//! This function determines if the table requires a homing operation in order
//! to gain (or re-gain) knowledge of the table position.  At startup, and
//! after an emergency stop, the position of the tool on the table is unknown.
//!
//! \return Returns a boolean that is \b true if the table requires a homing
//! operation and \b false if not.
//
//*****************************************************************************
tBoolean
TableNeedsHoming(void)
{
    //
    // Determine if the table requires a homing operation.
    //
    return((g_ulHomeState == STATE_HOMED) ? false : true);
}

//*****************************************************************************
//
//! Gets the current position of the tool.
//!
//! \param plX is a pointer to a long that is filled in with the X position,
//! specified in steps.
//! \param plY is a pointer to a long that is filled in with the Y position,
//! specified in steps.
//! \param plZ is a pointer to a long that is filled in with the Z position,
//! specified in steps.
//!
//! This function determines the current position of the tool on the table.
//! The position returned is only valid if the table is not in need of a homing
//! operation; otherwise, the position returned is negative one for each axis.
//!
//! \return None.
//
//*****************************************************************************
void
TableGetPosition(long *plX, long *plY, long *plZ)
{
    //
    // See if the tables requires a homing operation.
    //
    if(g_ulHomeState == STATE_HOMED)
    {
        //
        // Return the current position of the three axes.
        //
        *plX = g_sXAxis.lPos;
        *plY = g_sYAxis.lPos;
        *plZ = g_sZAxis.lPos;
    }
    else
    {
        //
        // The table requires a homing operation, so return -1 as the position
        // of each axis.
        //
        *plX = -1;
        *plY = -1;
        *plZ = -1;
    }
}

//*****************************************************************************
//
// Close the Doxygen group.
//! @}
//
//*****************************************************************************