lua/src/trajtrap.c

   1 /***
   2  trapezoidal velocity trajectory
   3  @module liba.trajtrap
   4 */
   5
   6 #include "trajtrap.h"
   7 #include "a/trajtrap.h"
   8
   9 /***
  10  constructor for trapezoidal velocity trajectory
  11  @treturn a.trajtrap trapezoidal velocity trajectory userdata
  12  @function new
  13 */
  14 int liba_trajtrap_new(lua_State *L)
  15 {
  16     a_trajtrap *const ctx = lua_newclass(L, a_trajtrap);
  17     a_zero(ctx, sizeof(a_trajtrap));
  18     lua_registry_get(L, liba_trajtrap_new);
  19     lua_setmetatable(L, -2);
  20     return 1;
  21 }
  22
  23 /***
  24  generate for trapezoidal velocity trajectory
  25  @tparam a.trajtrap ctx trapezoidal velocity trajectory userdata
  26  @tparam number vm defines the maximum velocity during system operation
  27  @tparam number ac defines the acceleration before constant velocity
  28  @tparam number de defines the acceleration after constant velocity
  29  @tparam number p0 defines the initial position
  30  @tparam number p1 defines the final position
  31  @tparam[opt] number v0 defines the initial velocity
  32  @tparam[opt] number v1 defines the final velocity
  33  @treturn number total duration
  34  @function gen
  35 */
  36 int liba_trajtrap_gen(lua_State *L)
  37 {
  38     a_float v0 = 0, v1 = 0;
  39     a_float vm, ac, de, p0, p1;
  40     int const top = lua_gettop(L);
  41     a_trajtrap *const ctx = (a_trajtrap *)lua_touserdata(L, 1);
  42     if (ctx)
  43     {
  44         vm = (a_float)luaL_checknumber(L, 2);
  45         ac = (a_float)luaL_checknumber(L, 3);
  46         de = (a_float)luaL_checknumber(L, 4);
  47         p0 = (a_float)luaL_checknumber(L, 5);
  48         p1 = (a_float)luaL_checknumber(L, 6);
  49         if (top >= 7) { v0 = (a_float)luaL_checknumber(L, 7); }
  50         if (top >= 8) { v1 = (a_float)luaL_checknumber(L, 8); }
  51         a_trajtrap_gen(ctx, vm, ac, de, p0, p1, v0, v1);
  52         lua_pushnumber(L, (lua_Number)ctx->t);
  53         return 1;
  54     }
  55     return 0;
  56 }
  57
  58 /***
  59  calculate position for trapezoidal velocity trajectory
  60  @tparam a.trajtrap ctx trapezoidal velocity trajectory userdata
  61  @tparam number x difference between current time and initial time
  62  @treturn number position output
  63  @function pos
  64 */
  65 int liba_trajtrap_pos(lua_State *L)
  66 {
  67     a_trajtrap const *const ctx = (a_trajtrap const *)lua_touserdata(L, 1);
  68     if (ctx)
  69     {
  70         a_float const x = (a_float)luaL_checknumber(L, 2);
  71         lua_pushnumber(L, (lua_Number)a_trajtrap_pos(ctx, x));
  72         return 1;
  73     }
  74     return 0;
  75 }
  76
  77 /***
  78  calculate velocity for trapezoidal velocity trajectory
  79  @tparam a.trajtrap ctx trapezoidal velocity trajectory userdata
  80  @tparam number x difference between current time and initial time
  81  @treturn number velocity output
  82  @function vel
  83 */
  84 int liba_trajtrap_vel(lua_State *L)
  85 {
  86     a_trajtrap const *const ctx = (a_trajtrap const *)lua_touserdata(L, 1);
  87     if (ctx)
  88     {
  89         a_float const x = (a_float)luaL_checknumber(L, 2);
  90         lua_pushnumber(L, (lua_Number)a_trajtrap_vel(ctx, x));
  91         return 1;
  92     }
  93     return 0;
  94 }
  95
  96 /***
  97  calculate acceleration for trapezoidal velocity trajectory
  98  @tparam a.trajtrap ctx trapezoidal velocity trajectory userdata
  99  @tparam number x difference between current time and initial time
 100  @treturn number acceleration output
 101  @function acc
 102 */
 103 int liba_trajtrap_acc(lua_State *L)
 104 {
 105     a_trajtrap const *const ctx = (a_trajtrap const *)lua_touserdata(L, 1);
 106     if (ctx)
 107     {
 108         a_float const x = (a_float)luaL_checknumber(L, 2);
 109         lua_pushnumber(L, (lua_Number)a_trajtrap_acc(ctx, x));
 110         return 1;
 111     }
 112     return 0;
 113 }
 114
 115 static int liba_trajtrap_set(lua_State *L)
 116 {
 117     switch (a_hash_bkdr(lua_tostring(L, 2), 0))
 118     {
 119     case 0xE8859EEB: // __name
 120     case 0xA65758B2: // __index
 121     case 0xAEB551C6: // __newindex
 122         break;
 123     default:
 124         lua_getmetatable(L, 1);
 125         lua_replace(L, 1);
 126         lua_rawset(L, 1);
 127     }
 128     return 0;
 129 }
 130
 131 static int liba_trajtrap_get(lua_State *L)
 132 {
 133     a_trajtrap const *const ctx = (a_trajtrap const *)lua_touserdata(L, 1);
 134     switch (a_hash_bkdr(lua_tostring(L, 2), 0))
 135     {
 136     case 0x00000074: // t
 137         lua_pushnumber(L, (lua_Number)ctx->t);
 138         break;
 139     case 0x00003980: // p0
 140         lua_pushnumber(L, (lua_Number)ctx->p0);
 141         break;
 142     case 0x00003981: // p1
 143         lua_pushnumber(L, (lua_Number)ctx->p1);
 144         break;
 145     case 0x00003C92: // v0
 146         lua_pushnumber(L, (lua_Number)ctx->v0);
 147         break;
 148     case 0x00003C93: // v1
 149         lua_pushnumber(L, (lua_Number)ctx->v1);
 150         break;
 151     case 0x00003CC5: // vc
 152         lua_pushnumber(L, (lua_Number)ctx->vc);
 153         break;
 154     case 0x00003BBD: // ta
 155         lua_pushnumber(L, (lua_Number)ctx->ta);
 156         break;
 157     case 0x00003BC0: // td
 158         lua_pushnumber(L, (lua_Number)ctx->td);
 159         break;
 160     case 0x000039B1: // pa
 161         lua_pushnumber(L, (lua_Number)ctx->pa);
 162         break;
 163     case 0x000039B4: // pd
 164         lua_pushnumber(L, (lua_Number)ctx->pd);
 165         break;
 166     case 0x00003206: // ac
 167         lua_pushnumber(L, (lua_Number)ctx->ac);
 168         break;
 169     case 0x00003391: // de
 170         lua_pushnumber(L, (lua_Number)ctx->de);
 171         break;
 172     case 0xA65758B2: // __index
 173         lua_registry_get(L, liba_trajtrap_new);
 174         lua_num_set(L, -1, "t", ctx->t);
 175         lua_num_set(L, -1, "p0", ctx->p0);
 176         lua_num_set(L, -1, "p1", ctx->p1);
 177         lua_num_set(L, -1, "v0", ctx->v0);
 178         lua_num_set(L, -1, "v1", ctx->v1);
 179         lua_num_set(L, -1, "vc", ctx->vc);
 180         lua_num_set(L, -1, "ta", ctx->ta);
 181         lua_num_set(L, -1, "td", ctx->td);
 182         lua_num_set(L, -1, "pa", ctx->pa);
 183         lua_num_set(L, -1, "pd", ctx->pd);
 184         lua_num_set(L, -1, "ac", ctx->ac);
 185         lua_num_set(L, -1, "de", ctx->de);
 186         break;
 187     default:
 188         lua_getmetatable(L, 1);
 189         lua_replace(L, 1);
 190         lua_rawget(L, 1);
 191     }
 192     return 1;
 193 }
 194
 195 static int liba_trajtrap_(lua_State *L)
 196 {
 197     lua_pushcfunction(L, liba_trajtrap_new);
 198     lua_replace(L, 1);
 199     lua_call(L, lua_gettop(L) - 1, 1);
 200     return 1;
 201 }
 202
 203 int luaopen_liba_trajtrap(lua_State *L)
 204 {
 205     static lua_fun const funcs[] = {
 206         {"new", liba_trajtrap_new},
 207         {"gen", liba_trajtrap_gen},
 208         {"pos", liba_trajtrap_pos},
 209         {"vel", liba_trajtrap_vel},
 210         {"acc", liba_trajtrap_acc},
 211     };
 212     lua_createtable(L, 0, A_LEN(funcs));
 213     lua_fun_reg(L, -1, funcs, A_LEN(funcs));
 214     lua_createtable(L, 0, 1);
 215     lua_fun_set(L, -1, "__call", liba_trajtrap_);
 216     lua_setmetatable(L, -2);
 217
 218     static lua_fun const metas[] = {
 219         {"__newindex", liba_trajtrap_set},
 220         {"__index", liba_trajtrap_get},
 221     };
 222     lua_createtable(L, 0, A_LEN(metas) + A_LEN(funcs) + 1);
 223     lua_fun_reg(L, -1, metas, A_LEN(metas));
 224     lua_fun_reg(L, -1, funcs, A_LEN(funcs));
 225     lua_str_set(L, -1, "__name", "a.trajtrap");
 226     lua_registry_set(L, liba_trajtrap_new);
 227
 228     return 1;
 229 }