buzzvm_state buzzvm_function_call(buzzvm_t vm,
const char* fname,
uint32_t argc) {
/* Reset the VM state if it's DONE */
if(vm->state == BUZZVM_STATE_DONE)
vm->state = BUZZVM_STATE_READY;
/* Don't continue if the VM has an error */
if(vm->state != BUZZVM_STATE_READY)
return vm->state;
/* Push the function name (return with error if not found) */
buzzvm_pushs(vm, buzzvm_string_register(vm, fname, 0));
/* Get associated symbol */
buzzvm_gload(vm);
/* Make sure it's a closure */
buzzvm_type_assert(vm, 1, BUZZTYPE_CLOSURE);
/* Move closure before arguments */
if(argc > 0) {
buzzdarray_insert(vm->stack,
buzzdarray_size(vm->stack) - argc - 1,
buzzvm_stack_at(vm, 1));
buzzvm_pop(vm);
}
/* Call the closure */
return buzzvm_closure_call(vm, argc);
}
// proper way to send loooong 1D array
int BuzzSetMap(buzzvm_t vm){
/* Make sure one parameter has been passed */
buzzvm_lnum_assert(vm, 1);
/* Get the parameter */
buzzvm_lload(vm, 1);
buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE); // matrix
/* Get the table */
buzzobj_t t = buzzvm_stack_at(vm, 1);
/* Copy the values into a vector */
std::vector<float> mat;
for(int32_t i = 0; i < buzzdict_size(t->t.value); ++i) {
/* Duplicate the table */
buzzvm_dup(vm);
/* Push the index */
buzzvm_pushi(vm, i);
/* Get the value */
buzzvm_tget(vm);
/* Store it in the vector (assume all values are float, no mistake...) */
mat.push_back((float)buzzvm_stack_at(vm, 1)->f.value);
/* Get rid of the value, now useless */
buzzvm_pop(vm);
}
/* Get a pointer to the controller */
buzzvm_pushs(vm, buzzvm_string_register(vm, "controller", 1));
buzzvm_gload(vm);
/* Copy data into the controller */
reinterpret_cast<CBuzzControllerFootBot*>(buzzvm_stack_at(vm, 1)->u.value)->SetMapParams(mat, Sqrt(buzzdict_size(t->t.value)));
/* Done with the function */
return buzzvm_ret0(vm);
}
int BuzzStopProcessing(buzzvm_t vm) {
/* Get pointer to the controller */
buzzvm_pushs(vm, buzzvm_string_register(vm, "controller", 1));
buzzvm_gload(vm);
/* Call function */
reinterpret_cast<CBuzzControllerEFootBot*>(buzzvm_stack_at(vm, 1)->u.value)->StopProcessing();
return buzzvm_ret0(vm);
}
static int BuzzCameraDisable(buzzvm_t vm) {
/* Get pointer to the controller */
buzzvm_pushs(vm, buzzvm_string_register(vm, "controller", 1));
buzzvm_gload(vm);
/* Call function */
reinterpret_cast<CBuzzControllerFootBot*>(buzzvm_stack_at(vm, 1)->u.value)->CameraDisable();
return buzzvm_ret0(vm);
}
static int BuzzTakeOff(buzzvm_t vm) {
/* Get pointer to the controller */
buzzvm_pushs(vm, buzzvm_string_register(vm, "controller", 1));
buzzvm_gload(vm);
/* Call function */
int cont = reinterpret_cast<CBuzzControllerSpiri*>(buzzvm_stack_at(vm, 1)->u.value)->TakeOff();
buzzvm_pushi(vm, cont);
return buzzvm_ret1(vm);
}
static int BuzzRotate(buzzvm_t vm) {
/* Push the yaw angle */
buzzvm_lload(vm, 1);
/* Create a new angle with that */
CRadians cYaw(buzzvm_stack_at(vm, 1)->f.value);
/* Get pointer to the controller */
buzzvm_pushs(vm, buzzvm_string_register(vm, "controller", 1));
buzzvm_gload(vm);
/* Call function */
reinterpret_cast<CBuzzControllerSpiri*>(buzzvm_stack_at(vm, 1)->u.value)->SetYaw(cYaw);
return buzzvm_ret0(vm);
}
int BuzzRemoveCS(buzzvm_t vm) {
buzzvm_lnum_assert(vm, 1);
buzzvm_lload(vm, 1);
buzzvm_type_assert(vm, 1, BUZZTYPE_INT);
int id = buzzvm_stack_at(vm, 1)->i.value;
buzzvm_pushs(vm, buzzvm_string_register(vm, "controller", 1));
buzzvm_gload(vm);
reinterpret_cast<CBuzzControllerFootBot*>(buzzvm_stack_at(vm, 1)->u.value)->RemoveCS(id);
return buzzvm_ret0(vm);
}
int BuzzSetArgosMap(buzzvm_t vm) {
buzzvm_lnum_assert(vm, 1);
buzzvm_lload(vm, 1);
buzzvm_type_assert(vm, 1, BUZZTYPE_STRING);
std::string text = buzzvm_stack_at(vm, 1)->s.value.str;
/* Get pointer to the controller */
buzzvm_pushs(vm, buzzvm_string_register(vm, "controller", 1));
buzzvm_gload(vm);
/* Call function */
//printf("Value: %s", buzzvm_stack_at(vm, 1)->s.value.str);
reinterpret_cast<CBuzzControllerFootBot*>(buzzvm_stack_at(vm, 1)->u.value)->SetArgosMap(text);
return buzzvm_ret0(vm);
}
int BuzzDebugTrajectoryClear(buzzvm_t vm) {
/*
* Possible signatures
* debug.trajectory.clear()
* deletes all the trajectory points
*/
/* Get pointer to controller user data */
buzzvm_pushs(vm, buzzvm_string_register(vm, "controller", 1));
buzzvm_gload(vm);
buzzvm_type_assert(vm, 1, BUZZTYPE_USERDATA);
CBuzzController& cContr = *reinterpret_cast<CBuzzController*>(buzzvm_stack_at(vm, 1)->u.value);
cContr.GetARGoSDebugInfo().TrajectoryClear();
return buzzvm_ret0(vm);
}
static int BuzzGoTo(buzzvm_t vm) {
/* Push the vector components */
buzzvm_lload(vm, 1);
buzzvm_lload(vm, 2);
/* Create a new vector with that */
CVector2 cDir(buzzvm_stack_at(vm, 2)->f.value,
buzzvm_stack_at(vm, 1)->f.value);
/* Get pointer to the controller */
buzzvm_pushs(vm, buzzvm_string_register(vm, "controller", 1));
buzzvm_gload(vm);
/* Call function */
reinterpret_cast<CBuzzControllerEFootBot*>(buzzvm_stack_at(vm, 1)->u.value)->SetWheelSpeedsFromVector(cDir);
return buzzvm_ret0(vm);
}
int buzzneighbors_add(buzzvm_t vm,
uint16_t robot,
float distance,
float azimuth,
float elevation) {
if(vm->state != BUZZVM_STATE_READY) return vm->state;
/* Get "neighbors" table */
buzzvm_pushs(vm, buzzvm_string_register(vm, "neighbors", 1));
buzzvm_gload(vm);
buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE);
buzzobj_t nbr = buzzvm_stack_at(vm, 1);
/* Get "data" field */
buzzvm_pushs(vm, buzzvm_string_register(vm, "data", 1));
buzzvm_tget(vm);
if(buzzvm_stack_at(vm, 1)->o.type == BUZZTYPE_NIL) {
/* Empty data, create a new table */
buzzvm_pop(vm);
buzzvm_push(vm, nbr);
buzzvm_pushs(vm, buzzvm_string_register(vm, "data", 1));
buzzvm_pusht(vm);
buzzobj_t data = buzzvm_stack_at(vm, 1);
buzzvm_tput(vm);
buzzvm_push(vm, data);
}
/* When we get here, the "data" table is on top of the stack */
/* Push robot id */
buzzvm_pushi(vm, robot);
/* Create entry table */
buzzobj_t entry = buzzheap_newobj(vm->heap, BUZZTYPE_TABLE);
/* Insert distance */
buzzvm_push(vm, entry);
buzzvm_pushs(vm, buzzvm_string_register(vm, "distance", 1));
buzzvm_pushf(vm, distance);
buzzvm_tput(vm);
/* Insert azimuth */
buzzvm_push(vm, entry);
buzzvm_pushs(vm, buzzvm_string_register(vm, "azimuth", 1));
buzzvm_pushf(vm, azimuth);
buzzvm_tput(vm);
/* Insert elevation */
buzzvm_push(vm, entry);
buzzvm_pushs(vm, buzzvm_string_register(vm, "elevation", 1));
buzzvm_pushf(vm, elevation);
buzzvm_tput(vm);
/* Save entry into data table */
buzzvm_push(vm, entry);
buzzvm_tput(vm);
return vm->state;
}
int BuzzDebugTrajectoryDisable(buzzvm_t vm) {
/*
* Possible signatures
* debug.trajectory.disable()
* disables trajectory tracking
*/
/* Get pointer to controller user data */
buzzvm_pushs(vm, buzzvm_string_register(vm, "controller", 1));
buzzvm_gload(vm);
buzzvm_type_assert(vm, 1, BUZZTYPE_USERDATA);
CBuzzController* pcContr = reinterpret_cast<CBuzzController*>(buzzvm_stack_at(vm, 1)->u.value);
/* Call method */
CBuzzController::DebugTrajectoryDisable(pcContr);
return buzzvm_ret0(vm);
}
int BuzzShowBorderRobots(buzzvm_t vm) {
buzzvm_lnum_assert(vm, 1);
buzzvm_lload(vm, 1);
buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE);
buzzobj_t t = buzzvm_stack_at(vm, 1);
buzzvm_pushs(vm, buzzvm_string_register(vm, "controller", 1));
buzzvm_gload(vm);
std::vector<float> v;
buzzdict_foreach(t->t.value, di_read_elem, &v);
reinterpret_cast<CBuzzControllerFootBot*>(buzzvm_stack_at(vm, 1)->u.value)->m_border_robot_ids = v;
return buzzvm_ret0(vm);
}
int BuzzSetWheelsFb(buzzvm_t vm) {
buzzvm_lnum_assert(vm, 2);
/* Push speeds */
buzzvm_lload(vm, 1); /* Left speed */
buzzvm_lload(vm, 2); /* Right speed */
buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT);
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
/* Get pointer to the controller */
buzzvm_pushs(vm, buzzvm_string_register(vm, "controller", 1));
buzzvm_gload(vm);
/* Call function */
reinterpret_cast<CBuzzControllerEFootBot*>(
buzzvm_stack_at(vm, 1)->u.value)->
SetWheels(buzzvm_stack_at(vm, 3)->f.value, /* Left speed */
buzzvm_stack_at(vm, 2)->f.value /* Right speed */
);
return buzzvm_ret0(vm);
}
int BuzzDebugPrint(buzzvm_t vm) {
/* Get pointer to controller user data */
buzzvm_pushs(vm, buzzvm_string_register(vm, "controller", 1));
buzzvm_gload(vm);
buzzvm_type_assert(vm, 1, BUZZTYPE_USERDATA);
CBuzzController& cContr = *reinterpret_cast<CBuzzController*>(buzzvm_stack_at(vm, 1)->u.value);
/* Fill message */
std::ostringstream oss;
for(UInt32 i = 1; i < buzzdarray_size(vm->lsyms->syms); ++i) {
buzzvm_lload(vm, i);
buzzobj_t o = buzzvm_stack_at(vm, 1);
buzzvm_pop(vm);
switch(o->o.type) {
case BUZZTYPE_NIL:
oss << "[nil]";
break;
case BUZZTYPE_INT:
oss << o->i.value;
break;
case BUZZTYPE_FLOAT:
oss << o->f.value;
break;
case BUZZTYPE_TABLE:
oss << "[table with " << (buzzdict_size(o->t.value)) << " elems]";
break;
case BUZZTYPE_CLOSURE:
if(o->c.value.isnative)
oss << "[n-closure @" << o->c.value.ref << "]";
else
oss << "[c-closure @" << o->c.value.ref << "]";
break;
case BUZZTYPE_STRING:
oss << o->s.value.str;
break;
case BUZZTYPE_USERDATA:
oss << "[userdata @" << o->u.value << "]";
break;
default:
break;
}
}
cContr.GetARGoSDebugInfo().Msg = oss.str();
return buzzvm_ret0(vm);
}
int BuzzSetLEDsFb(buzzvm_t vm) {
buzzvm_lnum_assert(vm, 3);
/* Push the color components */
buzzvm_lload(vm, 1);
buzzvm_lload(vm, 2);
buzzvm_lload(vm, 3);
buzzvm_type_assert(vm, 3, BUZZTYPE_INT);
buzzvm_type_assert(vm, 2, BUZZTYPE_INT);
buzzvm_type_assert(vm, 1, BUZZTYPE_INT);
/* Create a new color with that */
CColor cColor(buzzvm_stack_at(vm, 3)->i.value,
buzzvm_stack_at(vm, 2)->i.value,
buzzvm_stack_at(vm, 1)->i.value);
/* Get pointer to the controller */
buzzvm_pushs(vm, buzzvm_string_register(vm, "controller", 1));
buzzvm_gload(vm);
/* Call function */
reinterpret_cast<CBuzzControllerEFootBot*>(buzzvm_stack_at(vm, 1)->u.value)->SetLEDs(cColor);
return buzzvm_ret0(vm);
}
int BuzzSetPath(buzzvm_t vm){
buzzvm_lnum_assert(vm, 1);
/* Get the parameter */
buzzvm_lload(vm, 1);
buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE); // dictionary
buzzobj_t t = buzzvm_stack_at(vm, 1);
std::vector<CBuzzControllerFootBot::PathItem> pis;
for(int32_t i = 0; i < buzzdict_size(t->t.value); ++i) {
buzzvm_dup(vm);
buzzvm_pushi(vm, i);
buzzvm_tget(vm);
int id = 0; int parent = 0; float x = 0.0; float y = 0.0;
for(int32_t j = 0; j < buzzdict_size(buzzvm_stack_at(vm, 1)->t.value); ++j) {
buzzvm_dup(vm);
buzzvm_pushi(vm, j);
buzzvm_tget(vm);
if(j == 0){
id = (float)buzzvm_stack_at(vm, 1)->i.value;
} else if(j == 1){
parent = (float)buzzvm_stack_at(vm, 1)->i.value;
} else if (j == 2){
x = (float)buzzvm_stack_at(vm, 1)->f.value;
} else if (j == 3){
y = (float)buzzvm_stack_at(vm, 1)->f.value;
}
//fprintf(stdout, "%d %f \n", j, (float)buzzvm_stack_at(vm, 1)->f.value);
buzzvm_pop(vm);
}
pis.push_back(CBuzzControllerFootBot::PathItem(id, parent, x, y));
buzzvm_pop(vm);
}
/* Get a pointer to the controller */
buzzvm_pushs(vm, buzzvm_string_register(vm, "controller", 1));
buzzvm_gload(vm);
/* Copy data into the controller */
reinterpret_cast<CBuzzControllerFootBot*>(buzzvm_stack_at(vm, 1)->u.value)->Done(pis);
/* Done with the function */
return buzzvm_ret0(vm);
}
int BuzzDrawObstacles(buzzvm_t vm){
buzzvm_lnum_assert(vm, 1);
buzzvm_lload(vm, 1);
buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE); // dictionary
buzzobj_t t = buzzvm_stack_at(vm, 1);
std::vector<CBuzzControllerFootBot::Obstacle> obs;
for(int32_t i = 0; i < buzzdict_size(t->t.value); ++i) {
buzzvm_dup(vm);
buzzvm_pushi(vm, i);
buzzvm_tget(vm);
float x = 0.0; float y = 0.0; float radius = 0.0;
int type = 0;
for(int32_t j = 0; j < buzzdict_size(buzzvm_stack_at(vm, 1)->t.value); ++j) {
buzzvm_dup(vm);
buzzvm_pushi(vm, j);
buzzvm_tget(vm);
if(j == 0){
x = (float)buzzvm_stack_at(vm, 1)->f.value;
} else if(j == 1){
y = (float)buzzvm_stack_at(vm, 1)->f.value;
} else if (j == 2){
radius = (float)buzzvm_stack_at(vm, 1)->f.value;
} else if (j == 3){
type = (int) buzzvm_stack_at(vm, 1)->i.value;
}
//fprintf(stdout, "%d %f \n", j, (float)buzzvm_stack_at(vm, 1)->f.value);
buzzvm_pop(vm);
}
obs.push_back(CBuzzControllerFootBot::Obstacle(x, y, radius, type));
buzzvm_pop(vm);
}
buzzvm_pushs(vm, buzzvm_string_register(vm, "controller", 1));
buzzvm_gload(vm);
reinterpret_cast<CBuzzControllerFootBot*>(buzzvm_stack_at(vm, 1)->u.value)->ArgosDrawObstacles(obs);
return buzzvm_ret0(vm);
}
int BuzzShowCoordinateSystem(buzzvm_t vm) {
buzzvm_lnum_assert(vm, 5);
buzzvm_lload(vm, 1);
buzzvm_lload(vm, 2);
buzzvm_lload(vm, 3);
buzzvm_lload(vm, 4);
buzzvm_lload(vm, 5);
buzzvm_type_assert(vm, 5, BUZZTYPE_INT);
buzzvm_type_assert(vm, 4, BUZZTYPE_INT);
buzzvm_type_assert(vm, 3, BUZZTYPE_INT);
buzzvm_type_assert(vm, 2, BUZZTYPE_INT);
buzzvm_type_assert(vm, 1, BUZZTYPE_INT);
buzzvm_pushs(vm, buzzvm_string_register(vm, "controller", 1));
buzzvm_gload(vm);
reinterpret_cast<CBuzzControllerFootBot*>(buzzvm_stack_at(vm, 1)->u.value)->SetArgosCoordinateIDs(
buzzvm_stack_at(vm, 6)->i.value,
buzzvm_stack_at(vm, 5)->i.value,
buzzvm_stack_at(vm, 4)->i.value,
buzzvm_stack_at(vm, 3)->i.value,
buzzvm_stack_at(vm, 2)->i.value);
return buzzvm_ret0(vm);
}
static int BuzzGoToP(buzzvm_t vm) {
/* Push the vector components */
buzzvm_lload(vm, 1);
buzzvm_lload(vm, 2);
/* Create a new vector with that */
buzzobj_t tLinSpeed = buzzvm_stack_at(vm, 2);
buzzobj_t tAngSpeed = buzzvm_stack_at(vm, 1);
Real fLinSpeed = 0.0, fAngSpeed = 0.0;
if(tLinSpeed->o.type == BUZZTYPE_INT) fLinSpeed = tLinSpeed->i.value;
else if(tLinSpeed->o.type == BUZZTYPE_FLOAT) fLinSpeed = tLinSpeed->f.value;
else {
buzzvm_seterror(vm,
BUZZVM_ERROR_TYPE,
"gotop(linspeed,angspeed): expected %s, got %s in first argument",
buzztype_desc[BUZZTYPE_FLOAT],
buzztype_desc[tLinSpeed->o.type]
);
return vm->state;
}
if(tAngSpeed->o.type == BUZZTYPE_INT) fAngSpeed = tAngSpeed->i.value;
else if(tAngSpeed->o.type == BUZZTYPE_FLOAT) fAngSpeed = tAngSpeed->f.value;
else {
buzzvm_seterror(vm,
BUZZVM_ERROR_TYPE,
"gotop(linspeed,angspeed): expected %s, got %s in second argument",
buzztype_desc[BUZZTYPE_FLOAT],
buzztype_desc[tAngSpeed->o.type]
);
return vm->state;
}
CVector2 cDir(fLinSpeed, CRadians(fAngSpeed));
/* Get pointer to the controller */
buzzvm_pushs(vm, buzzvm_string_register(vm, "controller", 1));
buzzvm_gload(vm);
/* Call function */
reinterpret_cast<CBuzzControllerFootBot*>(buzzvm_stack_at(vm, 1)->u.value)->SetWheelSpeedsFromVector(cDir);
return buzzvm_ret0(vm);
}
static int BuzzGoToC(buzzvm_t vm) {
/* Push the vector components */
buzzvm_lload(vm, 1);
buzzvm_lload(vm, 2);
/* Create a new vector with that */
buzzobj_t tX = buzzvm_stack_at(vm, 2);
buzzobj_t tY = buzzvm_stack_at(vm, 1);
CVector2 cDir;
if(tX->o.type == BUZZTYPE_INT) cDir.SetX(tX->i.value);
else if(tX->o.type == BUZZTYPE_FLOAT) cDir.SetX(tX->f.value);
else {
buzzvm_seterror(vm,
BUZZVM_ERROR_TYPE,
"gotoc(x,y): expected %s, got %s in first argument",
buzztype_desc[BUZZTYPE_FLOAT],
buzztype_desc[tX->o.type]
);
return vm->state;
}
if(tY->o.type == BUZZTYPE_INT) cDir.SetY(tY->i.value);
else if(tY->o.type == BUZZTYPE_FLOAT) cDir.SetY(tY->f.value);
else {
buzzvm_seterror(vm,
BUZZVM_ERROR_TYPE,
"gotoc(x,y): expected %s, got %s in second argument",
buzztype_desc[BUZZTYPE_FLOAT],
buzztype_desc[tY->o.type]
);
return vm->state;
}
/* Get pointer to the controller */
buzzvm_pushs(vm, buzzvm_string_register(vm, "controller", 1));
buzzvm_gload(vm);
/* Call function */
reinterpret_cast<CBuzzControllerFootBot*>(buzzvm_stack_at(vm, 1)->u.value)->SetWheelSpeedsFromVector(cDir);
return buzzvm_ret0(vm);
}
buzzvm_state buzzvm_step(buzzvm_t vm) {
/* buzzvm_dump(vm); */
/* Can't execute if not ready */
if(vm->state != BUZZVM_STATE_READY) return vm->state;
/* Execute GC */
buzzheap_gc(vm);
/* Fetch instruction and (potential) argument */
uint8_t instr = vm->bcode[vm->pc];
/* Execute instruction */
switch(instr) {
case BUZZVM_INSTR_NOP: {
inc_pc();
break;
}
case BUZZVM_INSTR_DONE: {
buzzvm_done(vm);
break;
}
case BUZZVM_INSTR_PUSHNIL: {
inc_pc();
buzzvm_pushnil(vm);
break;
}
case BUZZVM_INSTR_DUP: {
inc_pc();
buzzvm_dup(vm);
break;
}
case BUZZVM_INSTR_POP: {
if(buzzvm_pop(vm) != BUZZVM_STATE_READY) return vm->state;
inc_pc();
break;
}
case BUZZVM_INSTR_RET0: {
if(buzzvm_ret0(vm) != BUZZVM_STATE_READY) return vm->state;
assert_pc(vm->pc);
break;
}
case BUZZVM_INSTR_RET1: {
if(buzzvm_ret1(vm) != BUZZVM_STATE_READY) return vm->state;
assert_pc(vm->pc);
break;
}
case BUZZVM_INSTR_ADD: {
buzzvm_add(vm);
inc_pc();
break;
}
case BUZZVM_INSTR_SUB: {
buzzvm_sub(vm);
inc_pc();
break;
}
case BUZZVM_INSTR_MUL: {
buzzvm_mul(vm);
inc_pc();
break;
}
case BUZZVM_INSTR_DIV: {
buzzvm_div(vm);
inc_pc();
break;
}
case BUZZVM_INSTR_MOD: {
buzzvm_mod(vm);
inc_pc();
break;
}
case BUZZVM_INSTR_POW: {
buzzvm_pow(vm);
inc_pc();
break;
}
case BUZZVM_INSTR_UNM: {
buzzvm_unm(vm);
inc_pc();
break;
}
case BUZZVM_INSTR_AND: {
buzzvm_and(vm);
inc_pc();
break;
}
case BUZZVM_INSTR_OR: {
buzzvm_or(vm);
inc_pc();
break;
}
case BUZZVM_INSTR_NOT: {
buzzvm_not(vm);
inc_pc();
break;
}
case BUZZVM_INSTR_EQ: {
buzzvm_eq(vm);
inc_pc();
break;
}
case BUZZVM_INSTR_NEQ: {
buzzvm_neq(vm);
inc_pc();
break;
}
case BUZZVM_INSTR_GT: {
buzzvm_gt(vm);
inc_pc();
break;
}
case BUZZVM_INSTR_GTE: {
buzzvm_gte(vm);
inc_pc();
break;
}
case BUZZVM_INSTR_LT: {
buzzvm_lt(vm);
inc_pc();
break;
}
case BUZZVM_INSTR_LTE: {
buzzvm_lte(vm);
inc_pc();
break;
}
case BUZZVM_INSTR_GLOAD: {
inc_pc();
buzzvm_gload(vm);
break;
}
case BUZZVM_INSTR_GSTORE: {
inc_pc();
if(buzzvm_gstore(vm) != BUZZVM_STATE_READY) return vm->state;
break;
}
case BUZZVM_INSTR_PUSHT: {
buzzvm_pusht(vm);
inc_pc();
break;
}
case BUZZVM_INSTR_TPUT: {
if(buzzvm_tput(vm) != BUZZVM_STATE_READY) return vm->state;
inc_pc();
break;
}
case BUZZVM_INSTR_TGET: {
if(buzzvm_tget(vm) != BUZZVM_STATE_READY) return vm->state;
inc_pc();
break;
}
case BUZZVM_INSTR_CALLC: {
inc_pc();
if(buzzvm_callc(vm) != BUZZVM_STATE_READY) return vm->state;
assert_pc(vm->pc);
break;
}
case BUZZVM_INSTR_CALLS: {
inc_pc();
if(buzzvm_calls(vm) != BUZZVM_STATE_READY) return vm->state;
assert_pc(vm->pc);
break;
}
case BUZZVM_INSTR_PUSHF: {
inc_pc();
get_arg(float);
if(buzzvm_pushf(vm, arg) != BUZZVM_STATE_READY) return vm->state;
break;
}
case BUZZVM_INSTR_PUSHI: {
inc_pc();
get_arg(int32_t);
if(buzzvm_pushi(vm, arg) != BUZZVM_STATE_READY) return vm->state;
break;
}
case BUZZVM_INSTR_PUSHS: {
inc_pc();
get_arg(int32_t);
if(buzzvm_pushs(vm, arg) != BUZZVM_STATE_READY) return vm->state;
break;
}
case BUZZVM_INSTR_PUSHCN: {
inc_pc();
get_arg(uint32_t);
if(buzzvm_pushcn(vm, arg) != BUZZVM_STATE_READY) return vm->state;
break;
}
case BUZZVM_INSTR_PUSHCC: {
inc_pc();
get_arg(uint32_t);
if(buzzvm_pushcc(vm, arg) != BUZZVM_STATE_READY) return vm->state;
break;
}
case BUZZVM_INSTR_PUSHL: {
inc_pc();
get_arg(uint32_t);
if(buzzvm_pushl(vm, arg) != BUZZVM_STATE_READY) return vm->state;
break;
}
case BUZZVM_INSTR_LLOAD: {
inc_pc();
get_arg(uint32_t);
buzzvm_lload(vm, arg);
break;
}
case BUZZVM_INSTR_LSTORE: {
inc_pc();
get_arg(uint32_t);
buzzvm_lstore(vm, arg);
break;
}
case BUZZVM_INSTR_JUMP: {
inc_pc();
get_arg(uint32_t);
vm->pc = arg;
assert_pc(vm->pc);
break;
}
case BUZZVM_INSTR_JUMPZ: {
inc_pc();
get_arg(uint32_t);
buzzvm_stack_assert(vm, 1);
if(buzzvm_stack_at(vm, 1)->o.type == BUZZTYPE_NIL ||
(buzzvm_stack_at(vm, 1)->o.type == BUZZTYPE_INT &&
buzzvm_stack_at(vm, 1)->i.value == 0)) {
vm->pc = arg;
assert_pc(vm->pc);
}
buzzvm_pop(vm);
break;
}
case BUZZVM_INSTR_JUMPNZ: {
inc_pc();
get_arg(uint32_t);
buzzvm_stack_assert(vm, 1);
if(buzzvm_stack_at(vm, 1)->o.type != BUZZTYPE_NIL &&
(buzzvm_stack_at(vm, 1)->o.type != BUZZTYPE_INT ||
buzzvm_stack_at(vm, 1)->i.value != 0)) {
vm->pc = arg;
assert_pc(vm->pc);
}
buzzvm_pop(vm);
break;
}
default:
buzzvm_seterror(vm, BUZZVM_ERROR_INSTR, NULL);
break;
}
return vm->state;
}
int BuzzDebugTrajectoryEnable(buzzvm_t vm) {
/*
* Possible signatures
* debug.trajectory.enable(maxpoints,r,g,b)
* enable trajectory tracking setting how many points should be stored and the drawing color
* debug.trajectory.enable(maxpoints)
* enable trajectory tracking setting how many points should be stored
* debug.trajectory.enable(r,g,b)
* enable trajectory tracking keeping maxpoints' last value and setting the drawing color
* debug.trajectory.enable()
* enable trajectory tracking keeping maxpoints' last value (default is 30)
*/
/* Get pointer to controller user data */
buzzvm_pushs(vm, buzzvm_string_register(vm, "controller", 1));
buzzvm_gload(vm);
buzzvm_type_assert(vm, 1, BUZZTYPE_USERDATA);
CBuzzController* pcContr = reinterpret_cast<CBuzzController*>(buzzvm_stack_at(vm, 1)->u.value);
/* Get last known value for max points */
SInt32 nMaxPoints = pcContr->GetARGoSDebugInfo().Trajectory.MaxPoints;
/* Parse arguments */
if(buzzvm_lnum(vm) == 4) {
/* Max points */
buzzvm_lload(vm, 1);
buzzvm_type_assert(vm, 1, BUZZTYPE_INT);
nMaxPoints = buzzvm_stack_at(vm, 1)->i.value;
/* RGB drawing color */
buzzvm_lload(vm, 2);
buzzvm_type_assert(vm, 1, BUZZTYPE_INT);
buzzvm_lload(vm, 3);
buzzvm_type_assert(vm, 1, BUZZTYPE_INT);
buzzvm_lload(vm, 4);
buzzvm_type_assert(vm, 1, BUZZTYPE_INT);
pcContr->GetARGoSDebugInfo().Trajectory.Color.Set(
buzzvm_stack_at(vm, 3)->i.value,
buzzvm_stack_at(vm, 2)->i.value,
buzzvm_stack_at(vm, 1)->i.value);
}
else if(buzzvm_lnum(vm) == 3) {
/* RGB drawing color */
buzzvm_lload(vm, 1);
buzzvm_type_assert(vm, 1, BUZZTYPE_INT);
buzzvm_lload(vm, 2);
buzzvm_type_assert(vm, 1, BUZZTYPE_INT);
buzzvm_lload(vm, 3);
buzzvm_type_assert(vm, 1, BUZZTYPE_INT);
pcContr->GetARGoSDebugInfo().Trajectory.Color.Set(
buzzvm_stack_at(vm, 3)->i.value,
buzzvm_stack_at(vm, 2)->i.value,
buzzvm_stack_at(vm, 1)->i.value);
}
else if(buzzvm_lnum(vm) == 1) {
/* Max points */
buzzvm_lload(vm, 1);
buzzvm_type_assert(vm, 1, BUZZTYPE_INT);
nMaxPoints = buzzvm_stack_at(vm, 1)->i.value;
}
else if(buzzvm_lnum(vm) != 0) {
/* Bomb out */
buzzvm_seterror(vm, BUZZVM_ERROR_LNUM, "expected 4, 3, or 1 arguments, but %" PRId64 " were passed", buzzvm_lnum(vm));
}
/* Call method */
CBuzzController::DebugTrajectoryEnable(pcContr, nMaxPoints);
return buzzvm_ret0(vm);
}
int BuzzDebugRayAdd(buzzvm_t vm) {
/*
* Possible signatures
* debug.rays.add(r,g,b, x,y,z)
* draws a ray from the reference point of the robot to (x,y,z).
* (x,y,z) is expressed wrt the robot reference frame
* debug.rays.add(r,g,b, x0,y0,z0, x1,y1,z1)
* draws a ray from (x0,y0,z0) to (x1,y1,z1)
* (x0,y0,z0) and (x1,y1,z1) are expressed wrt the robot reference frame
*/
CColor cColor;
CVector3 cStart, cEnd;
/* Parse arguments */
int64_t argn = buzzvm_lnum(vm);
if(argn == 6) {
/* Parse color */
buzzvm_lload(vm, 1); /* red */
buzzvm_type_assert(vm, 1, BUZZTYPE_INT);
buzzvm_lload(vm, 2); /* green */
buzzvm_type_assert(vm, 1, BUZZTYPE_INT);
buzzvm_lload(vm, 3); /* blue */
buzzvm_type_assert(vm, 1, BUZZTYPE_INT);
cColor.Set(buzzvm_stack_at(vm, 3)->i.value,
buzzvm_stack_at(vm, 2)->i.value,
buzzvm_stack_at(vm, 1)->i.value);
/* Parse end vector */
buzzvm_lload(vm, 4); /* x */
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
buzzvm_lload(vm, 5); /* y */
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
buzzvm_lload(vm, 6); /* z */
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
cEnd.Set(buzzvm_stack_at(vm, 3)->f.value,
buzzvm_stack_at(vm, 2)->f.value,
buzzvm_stack_at(vm, 1)->f.value);
}
else if(argn == 9) {
/* Parse color */
buzzvm_lload(vm, 1); /* red */
buzzvm_type_assert(vm, 1, BUZZTYPE_INT);
buzzvm_lload(vm, 2); /* green */
buzzvm_type_assert(vm, 1, BUZZTYPE_INT);
buzzvm_lload(vm, 3); /* blue */
buzzvm_type_assert(vm, 1, BUZZTYPE_INT);
cColor.Set(buzzvm_stack_at(vm, 3)->i.value,
buzzvm_stack_at(vm, 2)->i.value,
buzzvm_stack_at(vm, 1)->i.value);
/* Parse start vector */
buzzvm_lload(vm, 4); /* x */
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
buzzvm_lload(vm, 5); /* y */
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
buzzvm_lload(vm, 6); /* z */
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
cStart.Set(buzzvm_stack_at(vm, 3)->f.value,
buzzvm_stack_at(vm, 2)->f.value,
buzzvm_stack_at(vm, 1)->f.value);
/* Parse end vector */
buzzvm_lload(vm, 7); /* x */
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
buzzvm_lload(vm, 8); /* y */
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
buzzvm_lload(vm, 9); /* z */
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
cEnd.Set(buzzvm_stack_at(vm, 3)->f.value,
buzzvm_stack_at(vm, 2)->f.value,
buzzvm_stack_at(vm, 1)->f.value);
}
else {
/* Bomb out */
buzzvm_seterror(vm, BUZZVM_ERROR_LNUM, "expected 6 or 9 arguments, but %" PRId64 " were passed", buzzvm_lnum(vm));
}
/* Get pointer to controller user data */
buzzvm_pushs(vm, buzzvm_string_register(vm, "controller", 1));
buzzvm_gload(vm);
buzzvm_type_assert(vm, 1, BUZZTYPE_USERDATA);
CBuzzController& cContr = *reinterpret_cast<CBuzzController*>(buzzvm_stack_at(vm, 1)->u.value);
/* Call method */
cContr.GetARGoSDebugInfo().RayAdd(cColor, cStart, cEnd);
return buzzvm_ret0(vm);
}
