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