lua/src/pid_fuzzy.c

   1 /***
   2  fuzzy proportional integral derivative controller
   3  @module liba.pid_fuzzy
   4 */
   5
   6 #include "pid_fuzzy.h"
   7 #include "a/pid_fuzzy.h"
   8
   9 /***
  10  destructor for fuzzy PID controller
  11  @tparam a.pid_fuzzy ctx fuzzy PID controller userdata
  12  @function die
  13 */
  14 int liba_pid_fuzzy_die(lua_State *L)
  15 {
  16     a_pid_fuzzy *const ctx = (a_pid_fuzzy *)lua_touserdata(L, 1);
  17     if (ctx)
  18     {
  19         lua_alloc(L, ctx->me, 0);
  20         lua_alloc(L, ctx->mec, 0);
  21         lua_alloc(L, ctx->mkp, 0);
  22         lua_alloc(L, ctx->mki, 0);
  23         lua_alloc(L, ctx->mkd, 0);
  24         lua_alloc(L, a_pid_fuzzy_bfuzz(ctx), 0);
  25         ctx->me = 0;
  26         ctx->mec = 0;
  27         ctx->mkp = 0;
  28         ctx->mki = 0;
  29         ctx->mkd = 0;
  30         ctx->idx = 0;
  31         ctx->val = 0;
  32     }
  33     return 0;
  34 }
  35
  36 /***
  37  constructor for fuzzy PID controller
  38  @treturn a.pid_fuzzy fuzzy PID controller userdata
  39  @function new
  40 */
  41 int liba_pid_fuzzy_new(lua_State *L)
  42 {
  43     a_pid_fuzzy *const ctx = lua_newclass(L, a_pid_fuzzy);
  44     lua_registry_get(L, liba_pid_fuzzy_new);
  45     lua_setmetatable(L, -2);
  46     ctx->pid.kp = ctx->kp = 1;
  47     ctx->pid.ki = ctx->ki = 0;
  48     ctx->pid.kd = ctx->kd = 0;
  49     ctx->pid.summax = +A_FLOAT_INF;
  50     ctx->pid.summin = -A_FLOAT_INF;
  51     ctx->pid.outmax = +A_FLOAT_INF;
  52     ctx->pid.outmin = -A_FLOAT_INF;
  53     ctx->me = 0;
  54     ctx->mec = 0;
  55     ctx->mkp = 0;
  56     ctx->mki = 0;
  57     ctx->mkd = 0;
  58     ctx->idx = 0;
  59     ctx->val = 0;
  60     ctx->nrule = 0;
  61     ctx->nfuzz = 0;
  62     ctx->opr = a_fuzzy_equ;
  63     a_pid_fuzzy_init(ctx);
  64     return 1;
  65 }
  66
  67 /***
  68  initialize for fuzzy PID controller
  69  @tparam a.pid_fuzzy ctx fuzzy PID controller userdata
  70  @treturn a.pid_fuzzy fuzzy PID controller userdata
  71  @function init
  72 */
  73 int liba_pid_fuzzy_init(lua_State *L)
  74 {
  75     luaL_checktype(L, 1, LUA_TUSERDATA);
  76     a_pid_fuzzy *const ctx = (a_pid_fuzzy *)lua_touserdata(L, 1);
  77     ctx->pid.kp = ctx->kp = 1;
  78     ctx->pid.ki = ctx->ki = 0;
  79     ctx->pid.kd = ctx->kd = 0;
  80     ctx->pid.summax = +A_FLOAT_INF;
  81     ctx->pid.summin = -A_FLOAT_INF;
  82     ctx->pid.outmax = +A_FLOAT_INF;
  83     ctx->pid.outmin = -A_FLOAT_INF;
  84     ctx->opr = a_fuzzy_equ;
  85     a_pid_fuzzy_init(ctx);
  86     return 1;
  87 }
  88
  89 /***
  90  set fuzzy relational operator for fuzzy PID controller
  91  @tparam a.pid_fuzzy ctx fuzzy PID controller userdata
  92  @tparam int opr enumeration for fuzzy PID controller operator
  93  @treturn a.pid_fuzzy fuzzy PID controller userdata
  94  @function set_opr
  95 */
  96 int liba_pid_fuzzy_set_opr(lua_State *L)
  97 {
  98     a_pid_fuzzy *const ctx = (a_pid_fuzzy *)lua_touserdata(L, 1);
  99     if (ctx)
 100     {
 101         a_pid_fuzzy_set_opr(ctx, (unsigned int)luaL_checkinteger(L, 2));
 102         lua_pushvalue(L, 1);
 103         return 1;
 104     }
 105     return 0;
 106 }
 107
 108 /***
 109  set memory block for fuzzy PID controller
 110  @tparam a.pid_fuzzy ctx fuzzy PID controller userdata
 111  @tparam int num maximum number triggered by the rule
 112  @treturn a.pid_fuzzy fuzzy PID controller userdata
 113  @function set_nfuzz
 114 */
 115 int liba_pid_fuzzy_set_nfuzz(lua_State *L)
 116 {
 117     a_pid_fuzzy *const ctx = (a_pid_fuzzy *)lua_touserdata(L, 1);
 118     if (ctx)
 119     {
 120         unsigned int const num = (unsigned int)luaL_checkinteger(L, 2);
 121         void *ptr = lua_alloc(L, a_pid_fuzzy_bfuzz(ctx), A_PID_FUZZY_BFUZZ(num));
 122         a_pid_fuzzy_set_bfuzz(ctx, ptr, num);
 123         lua_pushvalue(L, 1);
 124         return 1;
 125     }
 126     return 0;
 127 }
 128
 129 /***
 130  set rule base for fuzzy PID controller
 131  @tparam a.pid_fuzzy ctx fuzzy PID controller userdata
 132  @tparam table me points to membership function parameter table, terminated by 0
 133  @tparam table mec points to membership function parameter table, terminated by 0
 134  @tparam table mkp points to Kp's rule base table which must be a square matrix
 135  @tparam table mki points to Ki's rule base table which must be a square matrix
 136  @tparam table mkd points to Kd's rule base table which must be a square matrix
 137  @treturn a.pid_fuzzy fuzzy PID controller userdata
 138  @function set_rule
 139 */
 140 int liba_pid_fuzzy_set_rule(lua_State *L)
 141 {
 142     a_pid_fuzzy *const ctx = (a_pid_fuzzy *)lua_touserdata(L, 1);
 143     if (ctx)
 144     {
 145         unsigned int const num = (unsigned int)lua_rawlen(L, 2);
 146         a_float const *const me = lua_table_num_get(L, 2, ctx->me, 0);
 147         a_float const *const mec = lua_table_num_get(L, 3, ctx->mec, 0);
 148         a_float const *const mkp = lua_table_num_get(L, 4, ctx->mkp, 0);
 149         a_float const *const mki = lua_table_num_get(L, 5, ctx->mki, 0);
 150         a_float const *const mkd = lua_table_num_get(L, 6, ctx->mkd, 0);
 151         a_pid_fuzzy_set_rule(ctx, num, me, mec, mkp, mki, mkd);
 152         lua_pushvalue(L, 1);
 153         return 1;
 154     }
 155     return 0;
 156 }
 157
 158 /***
 159  set proportional integral derivative constant for fuzzy PID controller
 160  @tparam a.pid_fuzzy ctx fuzzy PID controller userdata
 161  @tparam number kp proportional constant
 162  @tparam number ki integral constant
 163  @tparam number kd derivative constant
 164  @treturn a.pid_fuzzy fuzzy PID controller userdata
 165  @function set_kpid
 166 */
 167 int liba_pid_fuzzy_set_kpid(lua_State *L)
 168 {
 169     a_pid_fuzzy *const ctx = (a_pid_fuzzy *)lua_touserdata(L, 1);
 170     if (ctx)
 171     {
 172         a_float const kp = (a_float)luaL_checknumber(L, 2);
 173         a_float const ki = (a_float)luaL_checknumber(L, 3);
 174         a_float const kd = (a_float)luaL_checknumber(L, 4);
 175         a_pid_fuzzy_set_kpid(ctx, kp, ki, kd);
 176         lua_pushvalue(L, 1);
 177         return 1;
 178     }
 179     return 0;
 180 }
 181
 182 /***
 183  calculate for fuzzy PID controller
 184  @tparam a.pid_fuzzy ctx fuzzy PID controller userdata
 185  @tparam number set setpoint value
 186  @tparam number fdb feedback value
 187  @treturn number setpoint value
 188  @function run
 189 */
 190 int liba_pid_fuzzy_run(lua_State *L)
 191 {
 192     a_pid_fuzzy *const ctx = (a_pid_fuzzy *)lua_touserdata(L, 1);
 193     if (ctx)
 194     {
 195         a_float const set = (a_float)luaL_checknumber(L, 2);
 196         a_float const fdb = (a_float)luaL_checknumber(L, 3);
 197         lua_pushnumber(L, (lua_Number)a_pid_fuzzy_run(ctx, set, fdb));
 198         return 1;
 199     }
 200     return 0;
 201 }
 202
 203 /***
 204  calculate for positional fuzzy PID controller
 205  @tparam a.pid_fuzzy ctx fuzzy PID controller userdata
 206  @tparam number set setpoint value
 207  @tparam number fdb feedback value
 208  @treturn number output value
 209  @function pos
 210 */
 211 int liba_pid_fuzzy_pos(lua_State *L)
 212 {
 213     a_pid_fuzzy *const ctx = (a_pid_fuzzy *)lua_touserdata(L, 1);
 214     if (ctx)
 215     {
 216         a_float const set = (a_float)luaL_checknumber(L, 2);
 217         a_float const fdb = (a_float)luaL_checknumber(L, 3);
 218         lua_pushnumber(L, (lua_Number)a_pid_fuzzy_pos(ctx, set, fdb));
 219         return 1;
 220     }
 221     return 0;
 222 }
 223
 224 /***
 225  calculate for incremental fuzzy PID controller
 226  @tparam a.pid_fuzzy ctx fuzzy PID controller userdata
 227  @tparam number set setpoint value
 228  @tparam number fdb feedback value
 229  @treturn number output value
 230  @function inc
 231 */
 232 int liba_pid_fuzzy_inc(lua_State *L)
 233 {
 234     a_pid_fuzzy *const ctx = (a_pid_fuzzy *)lua_touserdata(L, 1);
 235     if (ctx)
 236     {
 237         a_float const set = (a_float)luaL_checknumber(L, 2);
 238         a_float const fdb = (a_float)luaL_checknumber(L, 3);
 239         lua_pushnumber(L, (lua_Number)a_pid_fuzzy_inc(ctx, set, fdb));
 240         return 1;
 241     }
 242     return 0;
 243 }
 244
 245 /***
 246  zeroing for fuzzy PID controller
 247  @tparam a.pid_fuzzy ctx fuzzy PID controller userdata
 248  @treturn a.pid_fuzzy fuzzy PID controller userdata
 249  @function zero
 250 */
 251 int liba_pid_fuzzy_zero(lua_State *L)
 252 {
 253     a_pid_fuzzy *const ctx = (a_pid_fuzzy *)lua_touserdata(L, 1);
 254     if (ctx)
 255     {
 256         a_pid_fuzzy_zero(ctx);
 257         return 1;
 258     }
 259     return 0;
 260 }
 261
 262 static int liba_pid_fuzzy_set(lua_State *L)
 263 {
 264     a_pid_fuzzy *const ctx = (a_pid_fuzzy *)lua_touserdata(L, 1);
 265     switch (a_hash_bkdr(lua_tostring(L, 2), 0))
 266     {
 267     case 0x00003731: // kp
 268         ctx->pid.kp = ctx->kp = (a_float)luaL_checknumber(L, 3);
 269         break;
 270     case 0x0000372A: // ki
 271         ctx->pid.ki = ctx->ki = (a_float)luaL_checknumber(L, 3);
 272         break;
 273     case 0x00003725: // kd
 274         ctx->pid.kd = ctx->kd = (a_float)luaL_checknumber(L, 3);
 275         break;
 276     case 0x10E9FF9D: // summax
 277         ctx->pid.summax = (a_float)luaL_checknumber(L, 3);
 278         break;
 279     case 0x10EA03AB: // summin
 280         ctx->pid.summin = (a_float)luaL_checknumber(L, 3);
 281         break;
 282     case 0x23C8F10E: // outmax
 283         ctx->pid.outmax = (a_float)luaL_checknumber(L, 3);
 284         break;
 285     case 0x23C8F51C: // outmin
 286         ctx->pid.outmin = (a_float)luaL_checknumber(L, 3);
 287         break;
 288     case 0x98AE2495: // nfuzz
 289     {
 290         unsigned int const num = (unsigned int)luaL_checkinteger(L, 3);
 291         void *ptr = lua_alloc(L, a_pid_fuzzy_bfuzz(ctx), A_PID_FUZZY_BFUZZ(num));
 292         a_pid_fuzzy_set_bfuzz(ctx, ptr, num);
 293         break;
 294     }
 295     case 0xE8859EEB: // __name
 296     case 0xE70C48C6: // __call
 297     case 0xA65758B2: // __index
 298     case 0xAEB551C6: // __newindex
 299         break;
 300     default:
 301         lua_getmetatable(L, 1);
 302         lua_replace(L, 1);
 303         lua_rawset(L, 1);
 304     }
 305     return 0;
 306 }
 307
 308 static int liba_pid_fuzzy_get(lua_State *L)
 309 {
 310     a_pid_fuzzy const *const ctx = (a_pid_fuzzy const *)lua_touserdata(L, 1);
 311     switch (a_hash_bkdr(lua_tostring(L, 2), 0))
 312     {
 313     case 0x00003731: // kp
 314         lua_pushnumber(L, (lua_Number)ctx->kp);
 315         break;
 316     case 0x0000372A: // ki
 317         lua_pushnumber(L, (lua_Number)ctx->ki);
 318         break;
 319     case 0x00003725: // kd
 320         lua_pushnumber(L, (lua_Number)ctx->kd);
 321         break;
 322     case 0x10E9FF9D: // summax
 323         lua_pushnumber(L, (lua_Number)ctx->pid.summax);
 324         break;
 325     case 0x10EA03AB: // summin
 326         lua_pushnumber(L, (lua_Number)ctx->pid.summin);
 327         break;
 328     case 0x001E5957: // sum
 329         lua_pushnumber(L, (lua_Number)ctx->pid.sum);
 330         break;
 331     case 0x23C8F10E: // outmax
 332         lua_pushnumber(L, (lua_Number)ctx->pid.outmax);
 333         break;
 334     case 0x23C8F51C: // outmin
 335         lua_pushnumber(L, (lua_Number)ctx->pid.outmin);
 336         break;
 337     case 0x001D4D3A: // out
 338         lua_pushnumber(L, (lua_Number)ctx->pid.out);
 339         break;
 340     case 0x001AE924: // fdb
 341         lua_pushnumber(L, (lua_Number)ctx->pid.fdb);
 342         break;
 343     case 0x001AAD55: // err
 344         lua_pushnumber(L, (lua_Number)ctx->pid.err);
 345         break;
 346     case 0x9A49C09A: // nrule
 347         lua_pushinteger(L, (lua_Integer)ctx->nrule);
 348         break;
 349     case 0x98AE2495: // nfuzz
 350         lua_pushinteger(L, (lua_Integer)ctx->nfuzz);
 351         break;
 352     case 0xA65758B2: // __index
 353         lua_registry_get(L, liba_pid_fuzzy_new);
 354         lua_num_set(L, -1, "kp", ctx->kp);
 355         lua_num_set(L, -1, "ki", ctx->ki);
 356         lua_num_set(L, -1, "kd", ctx->kd);
 357         lua_num_set(L, -1, "summax", ctx->pid.summax);
 358         lua_num_set(L, -1, "summin", ctx->pid.summin);
 359         lua_num_set(L, -1, "sum", ctx->pid.sum);
 360         lua_num_set(L, -1, "outmax", ctx->pid.outmax);
 361         lua_num_set(L, -1, "outmin", ctx->pid.outmin);
 362         lua_num_set(L, -1, "out", ctx->pid.out);
 363         lua_num_set(L, -1, "fdb", ctx->pid.fdb);
 364         lua_num_set(L, -1, "err", ctx->pid.err);
 365         lua_int_set(L, -1, "nrule", (lua_Integer)ctx->nrule);
 366         lua_int_set(L, -1, "nfuzz", (lua_Integer)ctx->nfuzz);
 367         break;
 368     default:
 369         lua_getmetatable(L, 1);
 370         lua_replace(L, 1);
 371         lua_rawget(L, 1);
 372     }
 373     return 1;
 374 }
 375
 376 static int liba_pid_fuzzy_(lua_State *L)
 377 {
 378     lua_pushcfunction(L, liba_pid_fuzzy_new);
 379     lua_replace(L, 1);
 380     lua_call(L, lua_gettop(L) - 1, 1);
 381     return 1;
 382 }
 383
 384 int luaopen_liba_pid_fuzzy(lua_State *L)
 385 {
 386     /***
 387      enumeration for fuzzy relational operator
 388      @field CAP         min(a,b)
 389      @field CAP_ALGEBRA a*b
 390      @field CAP_BOUNDED max(a+b-1,0)
 391      @field CUP         max(a,b)
 392      @field CUP_ALGEBRA a+b-a*b
 393      @field CUP_BOUNDED min(a+b,1)
 394      @field EQU         sqrt(a,b)*sqrt(1-(1-a)*(1-b))
 395      @table op
 396     */
 397     static lua_int const enums[] = {
 398         {"CAP", A_PID_FUZZY_CAP},
 399         {"CAP_ALGEBRA", A_PID_FUZZY_CAP_ALGEBRA},
 400         {"CAP_BOUNDED", A_PID_FUZZY_CAP_BOUNDED},
 401         {"CUP", A_PID_FUZZY_CUP},
 402         {"CUP_ALGEBRA", A_PID_FUZZY_CUP_ALGEBRA},
 403         {"CUP_BOUNDED", A_PID_FUZZY_CUP_BOUNDED},
 404         {"EQU", A_PID_FUZZY_EQU},
 405     };
 406     static lua_fun const funcs[] = {
 407         {"new", liba_pid_fuzzy_new},
 408         {"init", liba_pid_fuzzy_init},
 409         {"set_opr", liba_pid_fuzzy_set_opr},
 410         {"set_nfuzz", liba_pid_fuzzy_set_nfuzz},
 411         {"set_rule", liba_pid_fuzzy_set_rule},
 412         {"set_kpid", liba_pid_fuzzy_set_kpid},
 413         {"run", liba_pid_fuzzy_run},
 414         {"pos", liba_pid_fuzzy_pos},
 415         {"inc", liba_pid_fuzzy_inc},
 416         {"zero", liba_pid_fuzzy_zero},
 417     };
 418     lua_createtable(L, 0, A_LEN(enums) + A_LEN(funcs));
 419     lua_int_reg(L, -1, enums, A_LEN(enums));
 420     lua_fun_reg(L, -1, funcs, A_LEN(funcs));
 421     lua_createtable(L, 0, 1);
 422     lua_fun_set(L, -1, "__call", liba_pid_fuzzy_);
 423     lua_setmetatable(L, -2);
 424
 425     static lua_fun const metas[] = {
 426         {"__newindex", liba_pid_fuzzy_set},
 427         {"__index", liba_pid_fuzzy_get},
 428 #if defined(LUA_VERSION_NUM) && (LUA_VERSION_NUM > 503)
 429         {"__close", liba_pid_fuzzy_die},
 430 #endif /* LUA_VERSION_NUM */
 431         {"__gc", liba_pid_fuzzy_die},
 432     };
 433     lua_createtable(L, 0, A_LEN(metas) + A_LEN(funcs) + 1);
 434     lua_fun_reg(L, -1, metas, A_LEN(metas));
 435     lua_fun_reg(L, -1, funcs, A_LEN(funcs));
 436     lua_str_set(L, -1, "__name", "a.pid_fuzzy");
 437     lua_registry_set(L, liba_pid_fuzzy_new);
 438
 439     return 1;
 440 }