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