at_pid.c

CNC 的开放码,EMC2 V2.2.8版

        rtapi_snprintf(buf, HAL_NAME_LEN, "pid.%d.maxerrorI", id);
        error = hal_param_float_new(buf, HAL_RW, &(this->maxErrorI), compId);
    }

    if(!error){
        rtapi_snprintf(buf, HAL_NAME_LEN, "pid.%d.maxerrorD", id);
        error = hal_param_float_new(buf, HAL_RW, &(this->maxErrorD), compId);
    }

    if(!error){
        rtapi_snprintf(buf, HAL_NAME_LEN, "pid.%d.maxcmdD", id);
        error = hal_param_float_new(buf, HAL_RW, &(this->maxCmdD), compId);
    }

    if(!error){
        rtapi_snprintf(buf, HAL_NAME_LEN, "pid.%d.maxcmdDD", id);
        error = hal_param_float_new(buf, HAL_RW, &(this->maxCmdDd), compId);
    }

    if(!error){
        rtapi_snprintf(buf, HAL_NAME_LEN, "pid.%d.bias", id);
        error = hal_param_float_new(buf, HAL_RW, &(this->bias), compId);
    }

    if(!error){
        rtapi_snprintf(buf, HAL_NAME_LEN, "pid.%d.Pgain", id);
        error = hal_param_float_new(buf, HAL_RW, &(this->pGain), compId);
    }

    if(!error){
        rtapi_snprintf(buf, HAL_NAME_LEN, "pid.%d.Igain", id);
        error = hal_param_float_new(buf, HAL_RW, &(this->iGain), compId);
    }

    if(!error){
        rtapi_snprintf(buf, HAL_NAME_LEN, "pid.%d.Dgain", id);
        error = hal_param_float_new(buf, HAL_RW, &(this->dGain), compId);
    }

    if(!error){
        rtapi_snprintf(buf, HAL_NAME_LEN, "pid.%d.FF0", id);
        error = hal_param_float_new(buf, HAL_RW, &(this->ff0Gain), compId);
    }

    if(!error){
        rtapi_snprintf(buf, HAL_NAME_LEN, "pid.%d.FF1", id);
        error = hal_param_float_new(buf, HAL_RW, &(this->ff1Gain), compId);
    }

    if(!error){
        rtapi_snprintf(buf, HAL_NAME_LEN, "pid.%d.FF2", id);
        error = hal_param_float_new(buf, HAL_RW, &(this->ff2Gain), compId);
    }

    if(!error){
        rtapi_snprintf(buf, HAL_NAME_LEN, "pid.%d.maxoutput", id);
        error = hal_param_float_new(buf, HAL_RW, &(this->maxOutput), compId);
    }

    if(!error){
        rtapi_snprintf(buf, HAL_NAME_LEN, "pid.%d.tune-effort", id);
        error = hal_param_float_new(buf, HAL_RW, &(this->tuneEffort), compId);
    }

    if(!error){
        rtapi_snprintf(buf, HAL_NAME_LEN, "pid.%d.tune-cycles", id);
        error = hal_param_u32_new(buf, HAL_RW, &(this->tuneCycles), compId);
    }

    if(!error){
        rtapi_snprintf(buf, HAL_NAME_LEN, "pid.%d.tune-type", id);
        error = hal_param_u32_new(buf, HAL_RW, &(this->tuneType), compId);
    }

    // Export optional parameters.
    if(debug > 0){
        if(!error){
            rtapi_snprintf(buf, HAL_NAME_LEN, "pid.%d.errorI", id);
            error = hal_param_float_new(buf, HAL_RO, &(this->errorI), compId);
        }

        if(!error){
            rtapi_snprintf(buf, HAL_NAME_LEN, "pid.%d.errorD", id);
            error = hal_param_float_new(buf, HAL_RO, &(this->errorD), compId);
        }

        if(!error){
            rtapi_snprintf(buf, HAL_NAME_LEN, "pid.%d.commandD", id);
            error = hal_param_float_new(buf, HAL_RO, &(this->cmdD), compId);
        }

        if(!error){
            rtapi_snprintf(buf, HAL_NAME_LEN, "pid.%d.commandDD", id);
            error = hal_param_float_new(buf, HAL_RO, &(this->cmdDd), compId);
        }

        if(!error){
            rtapi_snprintf(buf, HAL_NAME_LEN, "pid.%d.ultimate-gain", id);
            error = hal_param_float_new(buf, HAL_RO, &(this->ultimateGain), compId);
        }

        if(!error){
            rtapi_snprintf(buf, HAL_NAME_LEN, "pid.%d.ultimate-period", id);
            error = hal_param_float_new(buf, HAL_RO, &(this->ultimatePeriod), compId);
        }
    }

    // Export functions.
    if(!error){
        rtapi_snprintf(buf, HAL_NAME_LEN, "pid.%d.do-pid-calcs", id);
        error = hal_export_funct(buf, Pid_Refresh, this, 1, 0, compId);
    }

    if(!error){
        *this->pEnable = 0;
        *this->pCommand = 0;
        *this->pFeedback = 0;
        *this->pError = 0;
        *this->pOutput = 0;
        *this->pTuneMode = 0;
        *this->pTuneStart = 0;
    }

    // Restore saved message level.
    rtapi_set_msg_level(msg);
    return(error);
}


/*
 * Perform an auto-tune operation. Sets up a limit cycle using the specified
 * tune effort. Averages the amplitude and period over the specifed number of
 * cycles. This characterizes the process and determines the ultimate gain
 * and period, which are then used to calculate PID.
 *
 * CO(t) = P * [ e(t) + 1/Ti * (f e(t)dt) - Td * (d/dt PV(t)) ]
 *
 * Pu = 4/PI * tuneEffort / responseAmplitude
 * Ti = 0.5 * responsePeriod
 * Td = 0.125 * responsePeriod
 *
 * P = 0.6 * Pu
 * I = P * 1/Ti
 * D = P * Td
 */
static void
Pid_AutoTune(Pid *this, long period)
{
    hal_float_t                 error;

    // Calculate the error.
    error = *this->pCommand - *this->pFeedback;
    *this->pError = error;

    // Check if enabled and if still in tune mode.
    if(!*this->pEnable || !*this->pTuneMode){
        this->state = STATE_TUNE_ABORT;
    }

    switch(this->state){
    case STATE_TUNE_IDLE:
        // Wait for tune start command.
        if(*this->pTuneStart)
            this->state = STATE_TUNE_START;
        break;

    case STATE_TUNE_START:
        // Initialize tuning variables and start limit cycle.
        this->state = STATE_TUNE_POS;
        this->cycleCount = 0;
        this->cyclePeriod = 0;
        this->cycleAmplitude = 0;
        this->totalTime = 0;
        this->avgAmplitude = 0;
        this->ultimateGain = 0;
        this->ultimatePeriod = 0;
        *this->pOutput = this->bias + fabs(this->tuneEffort);
        break;

    case STATE_TUNE_POS:
    case STATE_TUNE_NEG:
        this->cyclePeriod += period;

        if(error < 0.0){
            // Check amplitude.
            if(-error > this->cycleAmplitude)
                this->cycleAmplitude = -error;

            // Check for end of cycle.
            if(this->state == STATE_TUNE_POS){
                this->state = STATE_TUNE_NEG;
                Pid_CycleEnd(this);
            }

            // Update output so user can ramp effort until movement occurs.
            *this->pOutput = this->bias - fabs(this->tuneEffort);
        }else{
            // Check amplitude.
            if(error > this->cycleAmplitude)
                this->cycleAmplitude = error;

            // Check for end of cycle.
            if(this->state == STATE_TUNE_NEG){
                this->state = STATE_TUNE_POS;
                Pid_CycleEnd(this);
            }

            // Update output so user can ramp effort until movement occurs.
            *this->pOutput = this->bias + fabs(this->tuneEffort);
        }

        // Check if the last cycle just ended. This is really the number
        // of half cycles.
        if(this->cycleCount < this->tuneCycles)
            break;

        // Calculate PID.
        this->ultimateGain = (4.0 * fabs(this->tuneEffort))/(PI * this->avgAmplitude);
        this->ultimatePeriod = 2.0 * this->totalTime / this->tuneCycles;
        this->ff0Gain = 0;
        this->ff2Gain = 0;

        if(this->tuneType == TYPE_PID){
            // PID.
            this->pGain = 0.6 * this->ultimateGain;
            this->iGain = this->pGain / (this->ultimatePeriod / 2.0);
            this->dGain = this->pGain * (this->ultimatePeriod / 8.0);
            this->ff1Gain = 0;
        }else{
            // PI FF1.
            this->pGain = 0.45 * this->ultimateGain;
            this->iGain = this->pGain / (this->ultimatePeriod / 1.2);
            this->dGain = 0;

            // Scaling must be set so PID output is in user units per second.
            this->ff1Gain = 1;
        }

        // Fall through.

    case STATE_TUNE_ABORT:
    default:
        // Force output to zero.
        *this->pOutput = 0;

        // Abort any tuning cycle in progress.
        *this->pTuneStart = 0;
        this->state = (*this->pTuneMode)? STATE_TUNE_IDLE: STATE_PID;
    }
}


static void
Pid_CycleEnd(Pid *this)
{
    this->cycleCount++;
    this->avgAmplitude += this->cycleAmplitude / this->tuneCycles;
    this->cycleAmplitude = 0;
    this->totalTime += this->cyclePeriod * 0.000000001;
    this->cyclePeriod = 0;
}


/*
 * HAL EXPORTED FUNCTIONS
 */

static void
Pid_Refresh(void *arg, long periodNs)
{
    Pid                         *this = (Pid *)arg;
    hal_float_t                 period, periodRecip;
    hal_float_t                 prevCmdD, error, output;

    if(this->state != STATE_PID){
        Pid_AutoTune(this, periodNs);
        return;
    }

    // Calculate the error.
    error = *this->pCommand - *this->pFeedback;

    // Store error to error pin.
    *this->pError = error;

    // Check for tuning mode request.
    if(*this->pTuneMode){
        this->errorI = 0;
        this->prevError = 0;
        this->errorD = 0;
        this->prevCmd = 0;
        this->limitState = 0;
        this->cmdD = 0;
        this->cmdDd = 0;

        // Force output to zero.
        *this->pOutput = 0;

        // Switch to tuning mode.
        this->state = STATE_TUNE_IDLE;

        return;
    }

    // Precalculate some timing constants.
    period = periodNs * 0.000000001;
    periodRecip = 1.0 / period;

    // Apply error limits.
    if(this->maxError != 0.0){
        if(error > this->maxError){
            error = this->maxError;
        }else if(error < -this->maxError){
            error = -this->maxError;
        }
    }

    // Apply the deadband.
    if(error > this->deadband){
        error -= this->deadband;
    }else if(error < -this->deadband){
        error += this->deadband;
    }else{
        error = 0;
    }

    // Calculate derivative term.
    this->errorD = (error - this->prevError) * periodRecip;
    this->prevError = error;

    // Apply derivative limits.
    if(this->maxErrorD != 0.0){
        if(this->errorD > this->maxErrorD){
            this->errorD = this->maxErrorD;
        }else if(this->errorD < -this->maxErrorD){
            this->errorD = -this->maxErrorD;
        }
    }

    // Calculate derivative of command.
    // Save old value for 2nd derivative calc later.
    prevCmdD = this->cmdD;
    this->cmdD = (*this->pCommand - this->prevCmd) * periodRecip;
    this->prevCmd = *this->pCommand;

    // Apply derivative limits.
    if(this->maxCmdD != 0.0){
        if(this->cmdD > this->maxCmdD){
            this->cmdD = this->maxCmdD;
        }else if(this->cmdD < -this->maxCmdD){
            this->cmdD = -this->maxCmdD;
        }
    }

    // Calculate 2nd derivative of command.
    this->cmdDd = (this->cmdD - prevCmdD) * periodRecip;

    // Apply 2nd derivative limits.
    if(this->maxCmdDd != 0.0){
        if(this->cmdDd > this->maxCmdDd){
            this->cmdDd = this->maxCmdDd;
        }else if(this->cmdDd < -this->maxCmdDd){
            this->cmdDd = -this->maxCmdDd;
        }
    }

    // Check if enabled.
    if(!*this->pEnable){
        // Reset integrator.
        this->errorI = 0;

        // Force output to zero.
        *this->pOutput = 0;
        this->limitState = 0;

        return;
    }

    // If output is in limit, don't let integrator wind up.
    if(error * this->limitState <= 0.0){
        // Compute integral term.
        this->errorI += error * period;
    }

    // Apply integrator limits.
    if(this->maxErrorI != 0.0){
        if(this->errorI > this->maxErrorI){
            this->errorI = this->maxErrorI;
        }else if(this->errorI < -this->maxErrorI){
            this->errorI = -this->maxErrorI;
        }
    }

    // Calculate the output value.
    output =
        this->bias + this->pGain * error + this->iGain * this->errorI +
        this->dGain * this->errorD;
    output += *this->pCommand * this->ff0Gain + this->cmdD * this->ff1Gain +
        this->cmdDd * this->ff2Gain;

    // Apply output limits.
    if(this->maxOutput != 0.0){
        if(output > this->maxOutput){
            output = this->maxOutput;
            this->limitState = 1;
        }else if(output < -this->maxOutput){
            output = -this->maxOutput;
            this->limitState = -1;
        }else{
            this->limitState = 0;
        }
    }

    // Write final output value to output pin.
    *this->pOutput = output;
}