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