boss_plc.c

CNC 的开放码,EMC2 V2.2.8版

    hal_ready(component.id);
    return(0);
}


void
rtapi_app_exit(void)
{
    int                         i;
    Plc                         *pComp;

    hal_exit(component.id);

    for(i = 0; i < MAX_DEVICES; i++){
        if((pComp = component.plcTable[i]) != NULL){
            // TODO: Free device object when HAL supports free.
//            hal_free(pComp);
        }
    }
}


/******************************************************************************
 * PLC OBJECT FUNCTION DEFINITIONS
 ******************************************************************************/

/*
 * LOCAL FUNCTIONS
 */

static int
Plc_Init(Plc *this)
{
    int                         i;

    // Initialize variables.
    this->spindleState = SS_OFF;
    this->lastSpindleSpeed = 0.0;
    this->lastCycleStart = 1;

    // Initialize parameters.
    this->brakeOffDelay = 500;
    this->brakeOnDelay = 300;
    this->ampReadyDelay = 50;
    this->spindleLoToHi = 500;

    this->jogScale[0] = 0.0001;
    for(i = 1; i < NUM_JOG_SEL; i++){
        this->jogScale[i] = this->jogScale[i-1] * 10;
    }

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

    // Initialize limits.
    Limit_Init(&this->xLimit);
    Limit_Init(&this->yLimit);

    // Initialize amps.
    for(i = 0; i < NUM_AXIS; i++){
        Amp_Init(&this->amps[i]);
    }

    return(0);
}


static int
Plc_Export(Plc *this, int compId, int id)
{
    int                         msgLevel, error;
    char                        name[HAL_NAME_LEN + 2];

    // This function exports a lot of stuff, which results in a lot of
    // logging if msg_level is at INFO or ALL. So we save the current value
    // of msg_level and restore it later.  If you actually need to log this
    // function's actions, change the second line below.
    msgLevel = rtapi_get_msg_level();
    rtapi_set_msg_level(RTAPI_MSG_WARN);

    // Export pins and parameters.
    error = Plc_ExportFeed(this, compId, id, name);

    if(!error){
        error = Plc_ExportLimits(this, compId, id, name);
    }

    if(!error){
        error = Plc_ExportAmps(this, compId, id, name);
    }

    if(!error){
        error = Plc_ExportSpindle(this, compId, id, name);
    }
    if(!error){
        error = Plc_ExportJog(this, compId, id, name);
    }

    // Export functions.
    if(!error){
        rtapi_snprintf(name, HAL_NAME_LEN, "boss_plc.%d.refresh", id);
        error = hal_export_funct(name, Plc_Refresh, this, 1, 0, compId);
    }

    // Restore saved message level.
    rtapi_set_msg_level(msgLevel);

    return(error);
}


static int
Plc_ExportFeed(Plc *this, int compId, int id, char *name)
{
    int                         error;

    // Export pins.
    rtapi_snprintf(name, HAL_NAME_LEN, "boss_plc.%d.cycle-start-in", id);
    error = hal_pin_bit_new(name, HAL_IN, &this->pCycleStartIn, compId);

    if(!error){
        rtapi_snprintf(name, HAL_NAME_LEN, "boss_plc.%d.cycle-hold-in", id);
        error = hal_pin_bit_new(name, HAL_IN, &this->pCycleHoldIn, compId);
    }

    if(!error){
        rtapi_snprintf(name, HAL_NAME_LEN, "boss_plc.%d.feed-hold-out", id);
        error = hal_pin_bit_new(name, HAL_OUT, &this->pFeedHoldOut, compId);
    }

    if(!error){
        rtapi_snprintf(name, HAL_NAME_LEN, "boss_plc.%d.adaptive-feed-in", id);
        error = hal_pin_float_new(name, HAL_IN, &this->pAdaptiveFeedIn, compId);
    }

    if(!error){
        *this->pAdaptiveFeedIn = 1.0;
        rtapi_snprintf(name, HAL_NAME_LEN, "boss_plc.%d.adaptive-feed-out", id);
        error = hal_pin_float_new(name, HAL_OUT, &this->pAdaptiveFeedOut, compId);
    }

    if(!error){
        rtapi_snprintf(name, HAL_NAME_LEN, "boss_plc.%d.tool-change-in", id);
        error = hal_pin_bit_new(name, HAL_IN, &this->pToolChangeIn, compId);
    }

    if(!error){
        rtapi_snprintf(name, HAL_NAME_LEN, "boss_plc.%d.tool-changed-out", id);
        error = hal_pin_bit_new(name, HAL_OUT, &this->pToolChangedOut, compId);
    }

    if(!error){
        rtapi_snprintf(name, HAL_NAME_LEN, "boss_plc.%d.wait-user-out", id);
        error = hal_pin_bit_new(name, HAL_OUT, &this->pWaitUserOut, compId);
    }

    if(!error){
        rtapi_snprintf(name, HAL_NAME_LEN, "boss_plc.%d.mist-on-in", id);
        error = hal_pin_bit_new(name, HAL_IN, &this->pMistOnIn, compId);
    }

    if(!error){
        rtapi_snprintf(name, HAL_NAME_LEN, "boss_plc.%d.mist-on-out", id);
        error = hal_pin_bit_new(name, HAL_OUT, &this->pMistOnOut, compId);
    }

    if(!error){
        rtapi_snprintf(name, HAL_NAME_LEN, "boss_plc.%d.flood-on-in", id);
        error = hal_pin_bit_new(name, HAL_IN, &this->pFloodOnIn, compId);
    }

    if(!error){
        rtapi_snprintf(name, HAL_NAME_LEN, "boss_plc.%d.flood-on-out", id);
        error = hal_pin_bit_new(name, HAL_OUT, &this->pFloodOnOut, compId);
    }

    return(error);
}


static int
Plc_ExportLimits(Plc *this, int compId, int id, char *name)
{
    int                         error;

    // Export pins.
    rtapi_snprintf(name, HAL_NAME_LEN, "boss_plc.%d.limit-override-in", id);
    error = hal_pin_bit_new(name, HAL_IN, &this->pLimitOverrideIn, compId);

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

    if(!error){
        error = Limit_Export(&this->xLimit, compId, id, name, axisNames[0]);
    }

    if(!error){
        error = Limit_Export(&this->yLimit, compId, id, name, axisNames[1]);
    }

    if(!error){
        rtapi_snprintf(name, HAL_NAME_LEN, "boss_plc.%d.%c-limit-pos-in", id, axisNames[2]);
        error = hal_pin_bit_new(name, HAL_IN, &this->pZLimitPosIn, compId);
    }

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

    if(!error){
        rtapi_snprintf(name, HAL_NAME_LEN, "boss_plc.%d.%c-limit-neg-in", id, axisNames[2]);
        error = hal_pin_bit_new(name, HAL_IN, &this->pZLimitNegIn, compId);
    }

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

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

    // Export optional parameters.
    if(debug > 0){
        if(!error){
            rtapi_snprintf(name, HAL_NAME_LEN, "boss_plc.%d.%c-limit-state", id, axisNames[0]);
            error = hal_param_u32_new(name, HAL_RO, &this->xLimit.state, compId);
        }

        if(!error){
            rtapi_snprintf(name, HAL_NAME_LEN, "boss_plc.%d.%c-limit-state", id, axisNames[1]);
            error = hal_param_u32_new(name, HAL_RO, &this->yLimit.state, compId);
        }
    }

    return(error);
}


static int
Plc_ExportAmps(Plc *this, int compId, int id, char *name)
{
    int                         error, i;
    Amp                         *pAmp;

    rtapi_snprintf(name, HAL_NAME_LEN, "boss_plc.%d.amp-ready-delay", id);
    error = hal_param_u32_new(name, HAL_RW, &this->ampReadyDelay, compId);

    pAmp = this->amps;
    for(i = 0; i < NUM_AXIS && !error; i++, pAmp++){
        error = Amp_Export(pAmp, compId, id, name, axisNames[i]);
    }

    return(error);
}


static int
Plc_ExportSpindle(Plc *this, int compId, int id, char *name)
{
    int                         error;

    // Export parameters.
    rtapi_snprintf(name, HAL_NAME_LEN, "boss_plc.%d.brake-on-delay", id);
    error = hal_param_u32_new(name, HAL_RW, &this->brakeOnDelay, compId);

    if(!error){
        rtapi_snprintf(name, HAL_NAME_LEN, "boss_plc.%d.brake-off-delay", id);
        error = hal_param_u32_new(name, HAL_RW, &this->brakeOffDelay, compId);
    }

    if(!error){
        rtapi_snprintf(name, HAL_NAME_LEN, "boss_plc.%d.spindle-lo-to-hi", id);
        error = hal_param_float_new(name, HAL_RW, &this->spindleLoToHi, compId);
    }

    // Export optional parameters.
    if(debug > 0){
        if(!error){
            rtapi_snprintf(name, HAL_NAME_LEN, "boss_plc.%d.spindle-state", id);
            error = hal_param_u32_new(name, HAL_RO, &this->spindleState, compId);
        }
    }

    // Export pins.
    if(!error){
        rtapi_snprintf(name, HAL_NAME_LEN, "boss_plc.%d.spindle-speed-in", id);
        error = hal_pin_float_new(name, HAL_IN, &this->pSpindleSpeedIn, compId);
    }

    if(!error){
        rtapi_snprintf(name, HAL_NAME_LEN, "boss_plc.%d.spindle-is-on-in", id);
        error = hal_pin_bit_new(name, HAL_IN, &this->pSpindleIsOnIn, compId);
    }

    if(!error){
        rtapi_snprintf(name, HAL_NAME_LEN, "boss_plc.%d.spindle-fwd-out", id);
        error = hal_pin_bit_new(name, HAL_OUT, &this->pSpindleFwdOut, compId);
    }

    if(!error){
        rtapi_snprintf(name, HAL_NAME_LEN, "boss_plc.%d.spindle-rev-out", id);
        error = hal_pin_bit_new(name, HAL_OUT, &this->pSpindleRevOut, compId);
    }

    if(!error){
        rtapi_snprintf(name, HAL_NAME_LEN, "boss_plc.%d.spindle-inc-in", id);
        error = hal_pin_bit_new(name, HAL_IN, &this->pSpindleIncIn, compId);
    }

    if(!error){
        rtapi_snprintf(name, HAL_NAME_LEN, "boss_plc.%d.spindle-dec-in", id);
        error = hal_pin_bit_new(name, HAL_IN, &this->pSpindleDecIn, compId);
    }

    if(!error){
        rtapi_snprintf(name, HAL_NAME_LEN, "boss_plc.%d.spindle-inc-out", id);
        error = hal_pin_bit_new(name, HAL_OUT, &this->pSpindleIncOut, compId);
    }

    if(!error){
        rtapi_snprintf(name, HAL_NAME_LEN, "boss_plc.%d.spindle-dec-out", id);
        error = hal_pin_bit_new(name, HAL_OUT, &this->pSpindleDecOut, compId);
    }

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

    if(!error){
        rtapi_snprintf(name, HAL_NAME_LEN, "boss_plc.%d.brake-en-out", id);
        error = hal_pin_bit_new(name, HAL_OUT, &this->pBrakeEnOut, compId);
    }

    return(error);
}


static int
Plc_ExportJog(Plc *this, int compId, int id, char *name)
{
    int                         error, i;

    // Export parameters.
    for(i = 0, error = 0; i < NUM_JOG_SEL && !error; i++){
        rtapi_snprintf(name, HAL_NAME_LEN, "boss_plc.%d.jog-scale-%d", id, i);
        error = hal_param_float_new(name, HAL_RW, &this->jogScale[i], compId);
    }

    if(!error){
        for(i = 0; i < NUM_JOG_SEL && !error; i++){
            rtapi_snprintf(name, HAL_NAME_LEN, "boss_plc.%d.jog-sel-in-%d", id, i);
            error = hal_pin_bit_new(name, HAL_IN, &this->pJogSelIn[i], compId);
        }
    }

    // Export pins.
    if(!error){
        rtapi_snprintf(name, HAL_NAME_LEN, "boss_plc.%d.jog-scale-out", id);
        error = hal_pin_float_new(name, HAL_OUT, &this->pJogScaleOut, compId);
    }

    return(error);
}


/*
 * HAL EXPORTED FUNCTIONS
 */

static void
Plc_Refresh(void *arg, long period)
{
    Plc                         *this = (Plc *)arg;

    Plc_RefreshFeed(this, period);
    Plc_RefreshLimits(this, period);
    Plc_RefreshAmps(this, period);
    Plc_RefreshSpindle(this, period);
    Plc_RefreshJog(this, period);
}


static void
Plc_RefreshFeed(Plc *this, long period)
{
    BOOL                        riseCycleStart;

    riseCycleStart = !this->lastCycleStart && *this->pCycleStartIn;
    this->lastCycleStart = *this->pCycleStartIn;

    // Condition feed hold so machine waits for cycle start and spindle to be
    // running if it is enabled.
    *this->pFeedHoldOut = *this->pCycleHoldIn
                            || (*this->pSpindleSpeedIn && !*this->pSpindleIsOnIn)
                            || (*this->pSpindleIsOnIn
                                && (this->lastSpindleSpeed != *this->pSpindleSpeedIn))
                            || (*this->pFeedHoldOut && !riseCycleStart);
    this->lastSpindleSpeed = *this->pSpindleSpeedIn;

    // Limit rapid/feed to 1% when limits are being overriden.
    if(*this->pLimitOverrideIn && (*this->pAdaptiveFeedIn > 0.01))
        *this->pAdaptiveFeedOut = 0.01;