lua/src/pid.c

   1 /***
   2  proportional integral derivative controller
   3  @module liba.pid
   4 */
   5
   6 #include "pid.h"
   7 #include "a/pid.h"
   8
   9 /***
  10  constructor for PID controller
  11  @treturn a.pid PID controller userdata
  12  @function new
  13 */
  14 int liba_pid_new(lua_State *const L)
  15 {
  16     a_pid_s *const ctx = (a_pid_s *)lua_newuserdata(L, sizeof(a_pid_s));
  17     a_zero(ctx, sizeof(a_pid_s));
  18     lua_registry_get(L, liba_pid_new);
  19     lua_setmetatable(L, -2);
  20     ctx->kp = 1;
  21     ctx->summax = +A_FLOAT_INF;
  22     ctx->summin = -A_FLOAT_INF;
  23     ctx->outmax = +A_FLOAT_INF;
  24     ctx->outmin = -A_FLOAT_INF;
  25     a_pid_init(ctx);
  26     return 1;
  27 }
  28
  29 /***
  30  calculate for PID controller
  31  @tparam a.pid ctx PID controller userdata
  32  @treturn a.pid PID controller userdata
  33  @function init
  34 */
  35 int liba_pid_init(lua_State *const L)
  36 {
  37     luaL_checktype(L, 1, LUA_TUSERDATA);
  38     a_pid_s *const ctx = (a_pid_s *)lua_touserdata(L, 1);
  39     ctx->kp = 1;
  40     ctx->summax = +A_FLOAT_INF;
  41     ctx->summin = -A_FLOAT_INF;
  42     ctx->outmax = +A_FLOAT_INF;
  43     ctx->outmin = -A_FLOAT_INF;
  44     a_pid_init(ctx);
  45     return 1;
  46 }
  47
  48 /***
  49  zeroing for PID controller
  50  @tparam a.pid ctx PID controller userdata
  51  @treturn a.pid PID controller userdata
  52  @function zero
  53 */
  54 int liba_pid_zero(lua_State *const L)
  55 {
  56     a_pid_s *const ctx = (a_pid_s *)lua_touserdata(L, 1);
  57     if (ctx)
  58     {
  59         a_pid_zero(ctx);
  60         return 1;
  61     }
  62     return 0;
  63 }
  64
  65 /***
  66  set proportional integral derivative constant for PID controller
  67  @tparam a.pid ctx PID controller userdata
  68  @tparam number kp proportional constant
  69  @tparam number ki integral constant
  70  @tparam number kd derivative constant
  71  @treturn a.pid PID controller userdata
  72  @function kpid
  73 */
  74 int liba_pid_kpid(lua_State *const L)
  75 {
  76     a_pid_s *const ctx = (a_pid_s *)lua_touserdata(L, 1);
  77     if (ctx)
  78     {
  79         a_float_t const kp = (a_float_t)luaL_checknumber(L, 2);
  80         a_float_t const ki = (a_float_t)luaL_checknumber(L, 3);
  81         a_float_t const kd = (a_float_t)luaL_checknumber(L, 4);
  82         a_pid_kpid(ctx, kp, ki, kd);
  83         lua_pushvalue(L, 1);
  84         return 1;
  85     }
  86     return 0;
  87 }
  88
  89 /***
  90  calculate for PID controller
  91  @tparam a.pid ctx PID controller userdata
  92  @tparam number set setpoint value
  93  @tparam number fdb feedback value
  94  @treturn number setpoint value
  95  @function run
  96 */
  97 int liba_pid_run(lua_State *const L)
  98 {
  99     a_pid_s *const ctx = (a_pid_s *)lua_touserdata(L, 1);
 100     if (ctx)
 101     {
 102         a_float_t const set = (a_float_t)luaL_checknumber(L, 2);
 103         a_float_t const fdb = (a_float_t)luaL_checknumber(L, 3);
 104         lua_pushnumber(L, (lua_Number)a_pid_run(ctx, set, fdb));
 105         return 1;
 106     }
 107     return 0;
 108 }
 109
 110 /***
 111  calculate for positional PID controller
 112  @tparam a.pid ctx PID controller userdata
 113  @tparam number set setpoint value
 114  @tparam number fdb feedback value
 115  @treturn number output value
 116  @function pos
 117 */
 118 int liba_pid_pos(lua_State *const L)
 119 {
 120     a_pid_s *const ctx = (a_pid_s *)lua_touserdata(L, 1);
 121     if (ctx)
 122     {
 123         a_float_t const set = (a_float_t)luaL_checknumber(L, 2);
 124         a_float_t const fdb = (a_float_t)luaL_checknumber(L, 3);
 125         lua_pushnumber(L, (lua_Number)a_pid_pos(ctx, set, fdb));
 126         return 1;
 127     }
 128     return 0;
 129 }
 130
 131 /***
 132  calculate for incremental PID controller
 133  @tparam a.pid ctx PID controller userdata
 134  @tparam number set setpoint value
 135  @tparam number fdb feedback value
 136  @treturn number output value
 137  @function inc
 138 */
 139 int liba_pid_inc(lua_State *const L)
 140 {
 141     a_pid_s *const ctx = (a_pid_s *)lua_touserdata(L, 1);
 142     if (ctx)
 143     {
 144         a_float_t const set = (a_float_t)luaL_checknumber(L, 2);
 145         a_float_t const fdb = (a_float_t)luaL_checknumber(L, 3);
 146         lua_pushnumber(L, (lua_Number)a_pid_inc(ctx, set, fdb));
 147         return 1;
 148     }
 149     return 0;
 150 }
 151
 152 static int liba_pid_set(lua_State *const L)
 153 {
 154     char const *const field = lua_tostring(L, 2);
 155     a_pid_s *const ctx = (a_pid_s *)lua_touserdata(L, 1);
 156     a_u32_t const hash = (a_u32_t)a_hash_bkdr(field, 0);
 157     switch (hash)
 158     {
 159     case 0x00003731: // kp
 160         ctx->kp = (a_float_t)luaL_checknumber(L, 3);
 161         break;
 162     case 0x0000372A: // ki
 163         ctx->ki = (a_float_t)luaL_checknumber(L, 3);
 164         break;
 165     case 0x00003725: // kd
 166         ctx->kd = (a_float_t)luaL_checknumber(L, 3);
 167         break;
 168     case 0x10E9FF9D: // summax
 169         ctx->summax = (a_float_t)luaL_checknumber(L, 3);
 170         break;
 171     case 0x10EA03AB: // summin
 172         ctx->summin = (a_float_t)luaL_checknumber(L, 3);
 173         break;
 174     case 0x23C8F10E: // outmax
 175         ctx->outmax = (a_float_t)luaL_checknumber(L, 3);
 176         break;
 177     case 0x23C8F51C: // outmin
 178         ctx->outmin = (a_float_t)luaL_checknumber(L, 3);
 179         break;
 180     case 0xE8859EEB: // __name
 181     case 0xE70C48C6: // __call
 182     case 0xA65758B2: // __index
 183     case 0xAEB551C6: // __newindex
 184         break;
 185     default:
 186         lua_getmetatable(L, 1);
 187         lua_pushvalue(L, 3);
 188         lua_setfield(L, 4, field);
 189     }
 190     return 0;
 191 }
 192
 193 static int liba_pid_get(lua_State *const L)
 194 {
 195     char const *const field = lua_tostring(L, 2);
 196     a_pid_s const *const ctx = (a_pid_s const *)lua_touserdata(L, 1);
 197     a_u32_t const hash = (a_u32_t)a_hash_bkdr(field, 0);
 198     switch (hash)
 199     {
 200     case 0x00003731: // kp
 201         lua_pushnumber(L, (lua_Number)ctx->kp);
 202         break;
 203     case 0x0000372A: // ki
 204         lua_pushnumber(L, (lua_Number)ctx->ki);
 205         break;
 206     case 0x00003725: // kd
 207         lua_pushnumber(L, (lua_Number)ctx->kd);
 208         break;
 209     case 0x10E9FF9D: // summax
 210         lua_pushnumber(L, (lua_Number)ctx->summax);
 211         break;
 212     case 0x10EA03AB: // summin
 213         lua_pushnumber(L, (lua_Number)ctx->summin);
 214         break;
 215     case 0x23C8F10E: // outmax
 216         lua_pushnumber(L, (lua_Number)ctx->outmax);
 217         break;
 218     case 0x23C8F51C: // outmin
 219         lua_pushnumber(L, (lua_Number)ctx->outmin);
 220         break;
 221     case 0x001D4D3A: // out
 222         lua_pushnumber(L, (lua_Number)ctx->out);
 223         break;
 224     case 0x001AE924: // fdb
 225         lua_pushnumber(L, (lua_Number)ctx->fdb);
 226         break;
 227     case 0x001AAD55: // err
 228         lua_pushnumber(L, (lua_Number)ctx->err);
 229         break;
 230     case 0x001D0204: // new
 231         lua_pushcfunction(L, liba_pid_new);
 232         break;
 233     case 0x0E2ED8A0: // init
 234         lua_pushcfunction(L, liba_pid_init);
 235         break;
 236     case 0x1073A930: // zero
 237         lua_pushcfunction(L, liba_pid_zero);
 238         break;
 239     case 0x0E73F9D8: // kpid
 240         lua_pushcfunction(L, liba_pid_kpid);
 241         break;
 242     case 0x001E164F: // run
 243         lua_pushcfunction(L, liba_pid_run);
 244         break;
 245     case 0x001D8D30: // pos
 246         lua_pushcfunction(L, liba_pid_pos);
 247         break;
 248     case 0x001BB75E: // inc
 249         lua_pushcfunction(L, liba_pid_inc);
 250         break;
 251     case 0xA65758B2: // __index
 252         lua_registry_get(L, liba_pid_new);
 253         lua_num_set(L, -1, "kp", ctx->kp);
 254         lua_num_set(L, -1, "ki", ctx->ki);
 255         lua_num_set(L, -1, "kd", ctx->kd);
 256         lua_num_set(L, -1, "summax", ctx->summax);
 257         lua_num_set(L, -1, "summin", ctx->summin);
 258         lua_num_set(L, -1, "outmax", ctx->outmax);
 259         lua_num_set(L, -1, "outmin", ctx->outmin);
 260         lua_num_set(L, -1, "out", ctx->out);
 261         lua_num_set(L, -1, "fdb", ctx->fdb);
 262         lua_num_set(L, -1, "err", ctx->err);
 263         break;
 264     default:
 265         lua_getmetatable(L, 1);
 266         lua_getfield(L, 3, field);
 267     }
 268     return 1;
 269 }
 270
 271 static int liba_pid_(lua_State *const L)
 272 {
 273     lua_pushcfunction(L, liba_pid_new);
 274     lua_replace(L, 1);
 275     lua_call(L, lua_gettop(L) - 1, 1);
 276     return 1;
 277 }
 278
 279 int luaopen_liba_pid(lua_State *const L)
 280 {
 281     /***
 282      enumeration for PID controller
 283      @field RUN run and output setpoint
 284      @field POS positional PID controller
 285      @field INC incremental PID controller
 286      @table mode
 287     */
 288     static lua_int_s const enums[] = {
 289         {"RUN", A_PID_RUN},
 290         {"POS", A_PID_POS},
 291         {"INC", A_PID_INC},
 292     };
 293     static lua_fun_s const funcs[] = {
 294         {"new", liba_pid_new},
 295         {"init", liba_pid_init},
 296         {"zero", liba_pid_zero},
 297         {"kpid", liba_pid_kpid},
 298         {"run", liba_pid_run},
 299         {"pos", liba_pid_pos},
 300         {"inc", liba_pid_inc},
 301     };
 302     lua_createtable(L, 0, A_LEN(enums) + A_LEN(funcs));
 303     lua_int_reg(L, -1, enums, A_LEN(enums));
 304     lua_fun_reg(L, -1, funcs, A_LEN(funcs));
 305
 306     lua_createtable(L, 0, 1);
 307     lua_fun_set(L, -1, "__call", liba_pid_);
 308     lua_setmetatable(L, -2);
 309
 310     static lua_fun_s const metas[] = {
 311         {"__newindex", liba_pid_set},
 312         {"__index", liba_pid_get},
 313     };
 314     lua_createtable(L, 0, A_LEN(metas) + A_LEN(funcs) + 1);
 315     lua_fun_reg(L, -1, metas, A_LEN(metas));
 316     lua_fun_reg(L, -1, funcs, A_LEN(funcs));
 317     lua_str_set(L, -1, "__name", "a.pid");
 318
 319     lua_registry_set(L, liba_pid_new);
 320     return 1;
 321 }