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 = lua_newclass(L, 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     a_pid_fuzzy_s *const ctx = (a_pid_fuzzy_s *)lua_touserdata(L, 1);
 253     switch ((a_u32_t)a_hash_bkdr(lua_tostring(L, 2), 0))
 254     {
 255     case 0x00003731: // kp
 256         ctx->pid.kp = ctx->kp = (a_float_t)luaL_checknumber(L, 3);
 257         break;
 258     case 0x0000372A: // ki
 259         ctx->pid.ki = ctx->ki = (a_float_t)luaL_checknumber(L, 3);
 260         break;
 261     case 0x00003725: // kd
 262         ctx->pid.kd = ctx->kd = (a_float_t)luaL_checknumber(L, 3);
 263         break;
 264     case 0x10E9FF9D: // summax
 265         ctx->pid.summax = (a_float_t)luaL_checknumber(L, 3);
 266         break;
 267     case 0x10EA03AB: // summin
 268         ctx->pid.summin = (a_float_t)luaL_checknumber(L, 3);
 269         break;
 270     case 0x23C8F10E: // outmax
 271         ctx->pid.outmax = (a_float_t)luaL_checknumber(L, 3);
 272         break;
 273     case 0x23C8F51C: // outmin
 274         ctx->pid.outmin = (a_float_t)luaL_checknumber(L, 3);
 275         break;
 276     case 0x53A8DB2E: // joint
 277     {
 278         unsigned int const num = (unsigned int)luaL_checkinteger(L, 3);
 279         void *ptr = lua_alloc(L, a_pid_fuzzy_joint(ctx), A_PID_FUZZY_JOINT(num));
 280         a_pid_fuzzy_set_joint(ctx, ptr, num);
 281         break;
 282     }
 283     case 0xE8859EEB: // __name
 284     case 0xE70C48C6: // __call
 285     case 0xA65758B2: // __index
 286     case 0xAEB551C6: // __newindex
 287         break;
 288     default:
 289         lua_getmetatable(L, 1);
 290         lua_replace(L, 1);
 291         lua_rawset(L, 1);
 292     }
 293     return 0;
 294 }
 295
 296 static int liba_pid_fuzzy_get(lua_State *const L)
 297 {
 298     a_pid_fuzzy_s const *const ctx = (a_pid_fuzzy_s const *)lua_touserdata(L, 1);
 299     switch ((a_u32_t)a_hash_bkdr(lua_tostring(L, 2), 0))
 300     {
 301     case 0x00003731: // kp
 302         lua_pushnumber(L, (lua_Number)ctx->kp);
 303         break;
 304     case 0x0000372A: // ki
 305         lua_pushnumber(L, (lua_Number)ctx->ki);
 306         break;
 307     case 0x00003725: // kd
 308         lua_pushnumber(L, (lua_Number)ctx->kd);
 309         break;
 310     case 0x10E9FF9D: // summax
 311         lua_pushnumber(L, (lua_Number)ctx->pid.summax);
 312         break;
 313     case 0x10EA03AB: // summin
 314         lua_pushnumber(L, (lua_Number)ctx->pid.summin);
 315         break;
 316     case 0x23C8F10E: // outmax
 317         lua_pushnumber(L, (lua_Number)ctx->pid.outmax);
 318         break;
 319     case 0x23C8F51C: // outmin
 320         lua_pushnumber(L, (lua_Number)ctx->pid.outmin);
 321         break;
 322     case 0x001D4D3A: // out
 323         lua_pushnumber(L, (lua_Number)ctx->pid.out);
 324         break;
 325     case 0x001AE924: // fdb
 326         lua_pushnumber(L, (lua_Number)ctx->pid.fdb);
 327         break;
 328     case 0x001AAD55: // err
 329         lua_pushnumber(L, (lua_Number)ctx->pid.err);
 330         break;
 331     case 0xABD2FFCA: // order
 332         lua_pushinteger(L, (lua_Integer)ctx->order);
 333         break;
 334     case 0x53A8DB2E: // joint
 335         lua_pushinteger(L, (lua_Integer)ctx->joint);
 336         break;
 337     case 0xA65758B2: // __index
 338         lua_registry_get(L, liba_pid_fuzzy_new);
 339         lua_num_set(L, -1, "kp", ctx->kp);
 340         lua_num_set(L, -1, "ki", ctx->ki);
 341         lua_num_set(L, -1, "kd", ctx->kd);
 342         lua_num_set(L, -1, "summax", ctx->pid.summax);
 343         lua_num_set(L, -1, "summin", ctx->pid.summin);
 344         lua_num_set(L, -1, "outmax", ctx->pid.outmax);
 345         lua_num_set(L, -1, "outmin", ctx->pid.outmin);
 346         lua_num_set(L, -1, "out", ctx->pid.out);
 347         lua_num_set(L, -1, "fdb", ctx->pid.fdb);
 348         lua_num_set(L, -1, "err", ctx->pid.err);
 349         lua_int_set(L, -1, "order", (lua_Integer)ctx->order);
 350         lua_int_set(L, -1, "joint", (lua_Integer)ctx->joint);
 351         break;
 352     default:
 353         lua_getmetatable(L, 1);
 354         lua_replace(L, 1);
 355         lua_rawget(L, 1);
 356     }
 357     return 1;
 358 }
 359
 360 static int liba_pid_fuzzy_(lua_State *const L)
 361 {
 362     lua_pushcfunction(L, liba_pid_fuzzy_new);
 363     lua_replace(L, 1);
 364     lua_call(L, lua_gettop(L) - 1, 1);
 365     return 1;
 366 }
 367
 368 int luaopen_liba_pid_fuzzy(lua_State *const L)
 369 {
 370     /***
 371      enumeration for fuzzy relational operator
 372      @field CAP         min(a,b)
 373      @field CAP_ALGEBRA a*b
 374      @field CAP_BOUNDED max(a+b-1,0)
 375      @field CUP         max(a,b)
 376      @field CUP_ALGEBRA a+b-a*b
 377      @field CUP_BOUNDED min(a+b,1)
 378      @field EQU         sqrt(a,b)*sqrt(1-(1-a)*(1-b))
 379      @table op
 380     */
 381     static lua_int_s const enums[] = {
 382         {"CAP", A_PID_FUZZY_CAP},
 383         {"CAP_ALGEBRA", A_PID_FUZZY_CAP_ALGEBRA},
 384         {"CAP_BOUNDED", A_PID_FUZZY_CAP_BOUNDED},
 385         {"CUP", A_PID_FUZZY_CUP},
 386         {"CUP_ALGEBRA", A_PID_FUZZY_CUP_ALGEBRA},
 387         {"CUP_BOUNDED", A_PID_FUZZY_CUP_BOUNDED},
 388         {"EQU", A_PID_FUZZY_EQU},
 389     };
 390     static lua_fun_s const funcs[] = {
 391         {"new", liba_pid_fuzzy_new},
 392         {"init", liba_pid_fuzzy_init},
 393         {"zero", liba_pid_fuzzy_zero},
 394         {"op", liba_pid_fuzzy_op},
 395         {"rule", liba_pid_fuzzy_rule},
 396         {"set_joint", liba_pid_fuzzy_joint},
 397         {"kpid", liba_pid_fuzzy_kpid},
 398         {"run", liba_pid_fuzzy_run},
 399         {"pos", liba_pid_fuzzy_pos},
 400         {"inc", liba_pid_fuzzy_inc},
 401     };
 402     lua_createtable(L, 0, A_LEN(enums) + A_LEN(funcs));
 403     lua_int_reg(L, -1, enums, A_LEN(enums));
 404     lua_fun_reg(L, -1, funcs, A_LEN(funcs));
 405     lua_createtable(L, 0, 1);
 406     lua_fun_set(L, -1, "__call", liba_pid_fuzzy_);
 407     lua_setmetatable(L, -2);
 408
 409     static lua_fun_s const metas[] = {
 410         {"__newindex", liba_pid_fuzzy_set},
 411         {"__index", liba_pid_fuzzy_get},
 412 #if defined(LUA_VERSION_NUM) && (LUA_VERSION_NUM > 503)
 413         {"__close", liba_pid_fuzzy_die},
 414 #endif /* LUA_VERSION_NUM */
 415         {"__gc", liba_pid_fuzzy_die},
 416     };
 417     lua_createtable(L, 0, A_LEN(metas) + A_LEN(funcs) + 1);
 418     lua_fun_reg(L, -1, metas, A_LEN(metas));
 419     lua_fun_reg(L, -1, funcs, A_LEN(funcs));
 420     lua_str_set(L, -1, "__name", "a.pid_fuzzy");
 421     lua_registry_set(L, liba_pid_fuzzy_new);
 422
 423     return 1;
 424 }