boss_plc.c

CNC 的开放码,EMC2 V2.2.8版

    else
        *this->pAdaptiveFeedOut = *this->pAdaptiveFeedIn;

    // Wait for cycle start to acknowledge tool change.
    *this->pToolChangedOut = (*this->pToolChangeIn && riseCycleStart)
                            || (*this->pToolChangedOut && *this->pToolChangeIn);

    // Indicates waiting for user to press cycle start.
    *this->pWaitUserOut = *this->pFeedHoldOut
                            || (*this->pToolChangeIn && !*this->pToolChangedOut);

    // Turn coolant off during tool changes.
    *this->pMistOnOut = *this->pMistOnIn && !*this->pToolChangeIn;
    *this->pFloodOnOut = *this->pFloodOnIn && !*this->pToolChangeIn;
}


static void
Plc_RefreshLimits(Plc *this, long period)
{
    Limit_Refresh(&this->xLimit, *this->pLimitOverrideIn);
    Limit_Refresh(&this->yLimit, *this->pLimitOverrideIn);

    // Condition Z limits with override in manual mode.
    *this->pZLimitPosOut = *this->pZLimitPosIn
                            && !(*this->pZJogEnIn && *this->pLimitOverrideIn);
    *this->pZLimitNegOut = *this->pZLimitNegIn
                            && !(*this->pZJogEnIn && *this->pLimitOverrideIn);

    // Generate limit active signal for pilot lamp.
    *this->pLimitActiveOut = Limit_IsActive(&this->xLimit)
                            || Limit_IsActive(&this->yLimit)
                            || *this->pZLimitPosIn || *this->pZLimitNegIn;
}


static void
Plc_RefreshAmps(Plc *this, long period)
{
    int                         i;
    Amp                         *pAmp;

    pAmp = this->amps;
    for(i = 0; i < NUM_AXIS; i++, pAmp++){
        Amp_Refresh(pAmp, period, this->ampReadyDelay);
    }
}

static void
Plc_RefreshSpindle(Plc *this, long period)
{
    Timer_Update(&this->spindleTimer, period);

    switch(this->spindleState){
    // Spindle is off, brake is on.
    case SS_OFF:
        if(!*this->pBrakeEnIn){
            this->spindleState = SS_WAIT_BRAKE_OFF;
            *this->pBrakeEnOut = 0;
            Timer_SetTimeout(&this->spindleTimer, this->brakeOffDelay);
            Timer_Enable(&this->spindleTimer, TM_ONE_SHOT);
        }
        break;

    // Spindle is off, brake has been turned off. Wait at least a brake off
    // delay before turning spindle on.
    case SS_WAIT_BRAKE_OFF:
        if(*this->pBrakeEnIn){
            this->spindleState = SS_OFF;
            *this->pBrakeEnOut = 1;
            Timer_Disable(&this->spindleTimer);

        }else if((*this->pSpindleSpeedIn != 0.0)
                 && !Timer_IsEnabled(&this->spindleTimer)){

            this->spindleState = SS_WAIT_ON;

            if(*this->pSpindleSpeedIn > this->spindleLoToHi
              || (*this->pSpindleSpeedIn < 0.0
                && *this->pSpindleSpeedIn >= -this->spindleLoToHi)){

                *this->pSpindleFwdOut = 1;
            }else{
                *this->pSpindleRevOut = 1;
            }
        }
        break;

    // Spindle has been turned on. Wait for confirmation that it is running.
    case SS_WAIT_ON:
        if(*this->pSpindleIsOnIn){
            this->spindleState = SS_ON;

        }else if(*this->pSpindleSpeedIn == 0.0){
            this->spindleState = SS_WAIT_BRAKE_OFF;

            *this->pSpindleFwdOut = 0;
            *this->pSpindleRevOut = 0;
        }
        break;

    // Spindle is running.
    case SS_ON:
        if(*this->pSpindleSpeedIn == 0.0){
            this->spindleState = SS_WAIT_OFF;

            *this->pSpindleFwdOut = 0;
            *this->pSpindleRevOut = 0;
        }
        break;

    // Spindle has been turned off. Wait for confirmation.
    case SS_WAIT_OFF:
        if(!*this->pSpindleIsOnIn){
            this->spindleState = SS_WAIT_BRAKE_ON;

            Timer_SetTimeout(&this->spindleTimer, this->brakeOnDelay);
            Timer_Enable(&this->spindleTimer, TM_ONE_SHOT);
        }
        break;

    case SS_WAIT_BRAKE_ON:
        if(!Timer_IsEnabled(&this->spindleTimer)){
            this->spindleState = SS_WAIT_BRAKE_OFF;
        }
        break;

    default:
        this->spindleState = SS_WAIT_OFF;

        *this->pSpindleFwdOut = 0;
        *this->pSpindleRevOut = 0;
    }

    // Condition spindle increase and decrease so they are disabled when
    // spindle is not running and both cannot be enabled at the same time.
    *this->pSpindleIncOut = *this->pSpindleIncIn && !*this->pSpindleDecIn
                            && *this->pSpindleIsOnIn;
    *this->pSpindleDecOut = *this->pSpindleDecIn && !*this->pSpindleIncIn
                            && *this->pSpindleIsOnIn;
}


static void
Plc_RefreshJog(Plc *this, long period)
{
    int                         i;

    // Jog scale.
    for(i = 0; i < NUM_JOG_SEL; i++){
        if(*this->pJogSelIn[i]){
            *this->pJogScaleOut = this->jogScale[i];
            break;
        }
    }
}


/******************************************************************************
 * LIMIT OBJECT FUNCTION DEFINITIONS
 ******************************************************************************/

static int
Limit_Export(Limit *this, int compId, int id, char *name, char axis)
{
    int                         error;

    rtapi_snprintf(name, HAL_NAME_LEN, "boss_plc.%d.%c-position-in", id, axis);
    error = hal_pin_float_new(name, HAL_IN, &this->pPositionIn, compId);

    if(!error){
        rtapi_snprintf(name, HAL_NAME_LEN, "boss_plc.%d.%c-jog-en-in", id, axis);
        error = hal_pin_bit_new(name, HAL_IN, &this->pJogEnIn, compId);
    }

    if(!error){
        rtapi_snprintf(name, HAL_NAME_LEN, "boss_plc.%d.%c-limit-in", id, axis);
        error = hal_pin_bit_new(name, HAL_IN, &this->pIn, compId);
    }

    if(!error){
        rtapi_snprintf(name, HAL_NAME_LEN, "boss_plc.%d.%c-limit-pos-out", id, axis);
        error = hal_pin_bit_new(name, HAL_OUT, &this->pPosOut, compId);
    }

    if(!error){
        rtapi_snprintf(name, HAL_NAME_LEN, "boss_plc.%d.%c-limit-neg-out", id, axis);
        error = hal_pin_bit_new(name, HAL_OUT, &this->pNegOut, compId);
    }

    return(error);
}


static void
Limit_Init(Limit *this)
{
    this->state = LS_INIT;
}


static BOOL
Limit_IsActive(Limit *this)
{
    return(*this->pIn);
}

static void
Limit_Refresh(Limit *this, hal_bit_t override)
{
    switch(this->state){
    case LS_INIT:
    default:
        this->state = LS_ON_LIMIT;
        this->limitNeg = this->limitPos = 1;
        this->position = *this->pPositionIn;
        // Fall through.

    case LS_ON_LIMIT:
        if(*this->pIn == 0){
            this->limitNeg = this->limitPos = 0;

            if(*this->pPositionIn == this->position)
                this->state = LS_NO_MOTION;
            else if(*this->pPositionIn > this->position)
                this->state = LS_POS_MOTION;
            else if(*this->pPositionIn < this->position)
                this->state = LS_NEG_MOTION;
        }
        break;

    case LS_NO_MOTION:
        if(*this->pIn){
            this->state = LS_ON_LIMIT;
            this->limitNeg = this->limitPos = 1;
        }else if(*this->pPositionIn > this->position){
            this->state = LS_POS_MOTION;
        }else if(*this->pPositionIn < this->position){
            this->state = LS_NEG_MOTION;
        }
        break;

    case LS_POS_MOTION:
        if(*this->pIn){
            this->state = LS_ON_LIMIT;
            this->limitPos = 1;
        }else if(*this->pPositionIn == this->position){
            this->state = LS_NO_MOTION;
        }else if(*this->pPositionIn < this->position){
            this->state = LS_NEG_MOTION;
        }
        break;

    case LS_NEG_MOTION:
        if(*this->pIn){
            this->state = LS_ON_LIMIT;
            this->limitNeg = 1;
        }else if(*this->pPositionIn == this->position){
            this->state = LS_NO_MOTION;
        }else if(*this->pPositionIn > this->position){
            this->state = LS_POS_MOTION;
        }
        break;
    }

    this->position = *this->pPositionIn;

    // Condition limits with override in manual mode.
    *this->pPosOut = this->limitPos && !(*this->pJogEnIn && override);
    *this->pNegOut = this->limitNeg && !(*this->pJogEnIn && override);
}


/******************************************************************************
 * AMP OBJECT FUNCTION DEFINITIONS
 ******************************************************************************/

static int
Amp_Export(Amp *this, int compId, int id, char *name, char axis)
{
    int                         error;

    rtapi_snprintf(name, HAL_NAME_LEN, "boss_plc.%d.%c-amp-enable-in", id, axis);
    error = hal_pin_bit_new(name, HAL_IN, &this->pEnableIn, compId);

    if(!error){
        rtapi_snprintf(name, HAL_NAME_LEN, "boss_plc.%d.%c-amp-ready-in", id, axis);
        error = hal_pin_bit_new(name, HAL_IN, &this->pReadyIn, compId);
    }

    if(!error){
        rtapi_snprintf(name, HAL_NAME_LEN, "boss_plc.%d.%c-amp-fault-out", id, axis);
        error = hal_pin_bit_new(name, HAL_OUT, &this->pFaultOut, compId);
    }

    return(error);
}


static void
Amp_Init(Amp *this)
{
    // Initialize variables.
    this->lastEnable = 0;

    // Initialize timer.
    Timer_Init(&this->timer);
}


static void
Amp_Refresh(Amp *this, long period, hal_u32_t readyDelay)
{
    Timer_Update(&this->timer, period);

    if(*this->pEnableIn){
        if(!this->lastEnable){
            Timer_SetTimeout(&this->timer, readyDelay);
            Timer_Enable(&this->timer, TM_ONE_SHOT);
        }
    } else {
        Timer_Disable(&this->timer);
    }

    *this->pFaultOut = *this->pEnableIn && !*this->pReadyIn
                        && !Timer_IsEnabled(&this->timer);

    this->lastEnable = *this->pEnableIn;
}


/******************************************************************************
 * TIMER OBJECT FUNCTION DEFINITIONS
 ******************************************************************************/

static void
Timer_Init(Timer *this)
{
    this->enabled = FALSE;
    this->pTimeout = NULL;
}

static void
Timer_Enable(Timer *this, TimerMode mode)
{
    this->mode = mode;
    this->enabled = TRUE;
    this->count = 0;
}


static void
Timer_Disable(Timer *this)
{
    this->enabled = FALSE;
}


static BOOL
Timer_IsEnabled(Timer *this)
{
    return(this->enabled);
}


static void
Timer_Update(Timer *this, long period)
{
    if(!this->enabled)
        return;

    this->nSec += period;

    if(this->nSec > 1000000){
        this->count += this->nSec / 1000000;
        this->nSec %= 1000000;
    }

    if(this->count >= this->timeout){
        if(this->pTimeout != NULL)
            this->pTimeout(this->pArgs);

        if(this->mode != TM_CONTINUOUS){
            this->enabled = FALSE;
        }else{
            this->count -= this->timeout;
        }
    }
}


static void
Timer_SetTimeout(Timer *this, hal_u32_t timeout)
{
    this->count = 0;
    this->timeout = timeout;
}


#if 0
static void
Timer_SetCallback(Timer *this, TIMER_ROUTINE pCallback, void *pArgs)
{
    this->pTimeout = pCallback;
    this->pArgs = pArgs;
}
#endif