TMS320F28379D: C2000™︎ 微控制器论坛 C2000motorcontrol

static inline void SPDCALC_run(SPDCALC_Handle handle, float32_t theta)
{
    SPDCALC_Obj *obj = (SPDCALC_Obj *)handle;

    obj->ref  = theta;

    // error cal
    obj->err = obj->ref - obj->fbk;

    // roll in the error
    if(obj->err >= MATH_PI)
    {
        obj->err = obj->err - MATH_TWO_PI;
    }
    else if(obj->err <= -MATH_PI)
    {
        obj->err = obj->err + MATH_TWO_PI;
    }

    // P and I control
    obj->Up  = obj->Kp * obj->err;              // P control
    obj->Ui += obj->Ki * obj->err;              // I control
    obj->Ui  = __fsat(obj->Ui, obj->Umax, obj->Umin);

    // control output
    obj->out = obj->Up + obj->Ui;
    obj->out = __fsat(obj->out, obj->Umax, obj->Umin);      // rad/s

    obj->speed_Hz = obj->out * MATH_ONE_OVER_TWO_PI;        //?????? --add by jiaolu

    // Latest angle feedback estimation --> ( Fbk = integral of speed )
    obj->fbk = obj->fbk + obj->out * obj->thetaDelta;

    // roll "Fbk" within -pi to pi
    if(obj->fbk >= MATH_PI)
    {
        obj->fbk = obj->fbk - MATH_TWO_PI;
    }
    else if(obj->fbk <= -MATH_PI)
    {
        obj->fbk = obj->fbk + MATH_TWO_PI;
    }

    return;
}