// 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 buzzvm_swarm_select(buzzvm_t vm) {
buzzvm_lnum_assert(vm, 1);
/* Get the id */
buzzvm_lload(vm, 0);
buzzvm_pushs(vm, buzzvm_string_register(vm, "id"));
buzzvm_tget(vm);
uint16_t id = buzzvm_stack_at(vm, 1)->i.value;
/* Get the result of the condition check */
buzzvm_lload(vm, 1);
buzzvm_type_assert(vm, 1, BUZZTYPE_INT);
uint8_t in = buzzvm_stack_at(vm, 1)->i.value;
/* Update the swarm, if known */
if(buzzdict_exists(vm->swarms, &id)) {
/* Store membership */
buzzdict_set(vm->swarms, &id, &in);
/* Send update */
buzzoutmsg_queue_append_swarm_joinleave(
vm->outmsgs,
in ? BUZZMSG_SWARM_JOIN : BUZZMSG_SWARM_LEAVE,
id);
/* Return */
return buzzvm_ret0(vm);
}
else {
vm->state = BUZZVM_STATE_ERROR;
vm->error = BUZZVM_ERROR_SWARM;
return BUZZVM_STATE_ERROR;
}
}
int buzzvm_vstig_setonconflictlost(struct buzzvm_s* vm) {
buzzvm_lnum_assert(vm, 1);
/* Get vstig id */
buzzvm_lload(vm, 0);
buzzvm_pushs(vm, buzzvm_string_register(vm, "id"));
buzzvm_tget(vm);
uint16_t id = buzzvm_stack_at(vm, 1)->i.value;
/* Look for virtual stigmergy */
buzzvstig_t* vs = buzzdict_get(vm->vstigs, &id, buzzvstig_t);
if(vs) {
/* Virtual stigmergy found */
/* Get closure */
buzzvm_lload(vm, 1);
buzzvm_type_assert(vm, 1, BUZZTYPE_CLOSURE);
/* Clone the closure */
if((*vs)->onconflictlost) free((*vs)->onconflictlost);
(*vs)->onconflictlost = buzzobj_clone(buzzvm_stack_at(vm, 1));
}
else {
/* No virtual stigmergy found, just push false */
/* If this happens, its a bug */
buzzvm_pushnil(vm);
fprintf(stderr, "[BUG] [ROBOT %u] Can't find virtual stigmergy %u\n", vm->robot, id);
}
/* Return the value found */
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);
}
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);
}
int buzzneighbors_ignore(buzzvm_t vm) {
buzzvm_lnum_assert(vm, 1);
/* Get value id argument */
buzzvm_lload(vm, 1);
buzzvm_type_assert(vm, 1, BUZZTYPE_STRING);
/* Remove listener */
buzzdict_remove(
vm->listeners,
&(buzzvm_stack_at(vm, 1)->s.value.sid));
return buzzvm_ret0(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 buzzvm_swarm_exec(buzzvm_t vm) {
buzzvm_lnum_assert(vm, 1);
/* Get the id */
buzzvm_lload(vm, 0);
buzzvm_pushs(vm, buzzvm_string_register(vm, "id"));
buzzvm_tget(vm);
uint16_t id = buzzvm_stack_at(vm, 1)->i.value;
/* Get the swarm entry */
uint8_t* x = buzzdict_get(vm->swarms, &id, uint8_t);
if(!x) {
vm->state = BUZZVM_STATE_ERROR;
vm->error = BUZZVM_ERROR_SWARM;
return BUZZVM_STATE_ERROR;
}
/* Check whether the robot is in the swarm */
if(*x) {
/* Get the closure */
buzzvm_lload(vm, 1);
buzzvm_type_assert(vm, 1, BUZZTYPE_CLOSURE);
buzzobj_t c = buzzvm_stack_at(vm, 1);
/* Get rid of the current call structure */
if(buzzvm_ret0(vm) != BUZZVM_STATE_READY) return vm->state;
/* Save the current stack depth */
uint32_t stacks = buzzdarray_size(vm->stacks);
/* Push the current swarm in the stack */
buzzdarray_push(vm->swarmstack, &id);
/* Call the closure */
buzzvm_push(vm, c);
int32_t numargs = 0;
buzzvm_pushi(vm, numargs);
buzzvm_calls(vm);
do if(buzzvm_step(vm) != BUZZVM_STATE_READY) return vm->state;
while(stacks < buzzdarray_size(vm->stacks));
return vm->state;
}
else {
/* Get rid of the current call structure */
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_broadcast(buzzvm_t vm) {
buzzvm_lnum_assert(vm, 2);
/* Get value id argument */
buzzvm_lload(vm, 1);
buzzvm_type_assert(vm, 1, BUZZTYPE_STRING);
/* Get value argument */
buzzvm_lload(vm, 2);
/* Queue a message with (value_id, value) */
buzzoutmsg_queue_append_broadcast(
vm,
buzzvm_stack_at(vm, 2),
buzzvm_stack_at(vm, 1));
return buzzvm_ret0(vm);
}
int buzzvstig_put(buzzvm_t vm) {
buzzvm_lnum_assert(vm, 2);
/* Get vstig id */
id_get();
/* Get key */
buzzvm_lload(vm, 1);
buzzobj_t k = buzzvm_stack_at(vm, 1);
/* Get value */
buzzvm_lload(vm, 2);
buzzobj_t v = buzzvm_stack_at(vm, 1);
/* Look for virtual stigmergy */
const buzzvstig_t* vs = buzzdict_get(vm->vstigs, &id, buzzvstig_t);
if(vs) {
/* Look for the element */
const buzzvstig_elem_t* x = buzzvstig_fetch(*vs, &k);
if(x) {
/* Element found */
if(v->o.type != BUZZTYPE_NIL) {
/* New value is not nil, update the existing element */
(*x)->data = v;
++((*x)->timestamp);
(*x)->robot = vm->robot;
/* Append a PUT message to the out message queue */
buzzoutmsg_queue_append_vstig(vm, BUZZMSG_VSTIG_PUT, id, k, *x);
}
else {
/* New value is nil, must delete the existing element */
/* Make a new element with nil as value to update neighbors */
buzzvstig_elem_t y = buzzvstig_elem_new(
buzzobj_new(BUZZTYPE_NIL), // nil value
(*x)->timestamp + 1, // new timestamp
vm->robot); // robot id
/* Append a PUT message to the out message queue with nil in it */
buzzoutmsg_queue_append_vstig(vm, BUZZMSG_VSTIG_PUT, id, k, y);
/* Delete the existing element */
buzzvstig_remove(*vs, &k);
}
}
else if(v->o.type != BUZZTYPE_NIL) {
/* Element not found and new value is not nil, store it */
buzzvstig_elem_t y = buzzvstig_elem_new(v, 1, vm->robot);
buzzvstig_store(*vs, &k, &y);
/* Append a PUT message to the out message queue */
buzzoutmsg_queue_append_vstig(vm, BUZZMSG_VSTIG_PUT, id, k, y);
}
}
/* Return */
return buzzvm_ret0(vm);
}
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 buzzneighbors_listen(buzzvm_t vm) {
buzzvm_lnum_assert(vm, 2);
/* Get value id argument */
buzzvm_lload(vm, 1);
buzzvm_type_assert(vm, 1, BUZZTYPE_STRING);
/* Get listener argument */
buzzvm_lload(vm, 2);
buzzvm_type_assert(vm, 1, BUZZTYPE_CLOSURE);
/* Install listener */
buzzdict_set(
vm->listeners,
&buzzvm_stack_at(vm, 2)->s.value.sid,
&buzzvm_stack_at(vm, 1));
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 buzzvstig_foreach(struct buzzvm_s* vm) {
/* Make sure you got one argument */
buzzvm_lnum_assert(vm, 1);
/* Get vstig id */
id_get();
/* Look for virtual stigmergy */
const buzzvstig_t* vs = buzzdict_get(vm->vstigs, &id, buzzvstig_t);
if(vs) {
/* Virtual stigmergy found */
/* Get closure */
buzzvm_lload(vm, 1);
buzzvm_type_assert(vm, 1, BUZZTYPE_CLOSURE);
buzzobj_t c = buzzvm_stack_at(vm, 1);
/* Go through the elements and apply the closure */
struct buzzvstig_foreach_params p = { .vm = vm, .fun = c };
buzzdict_foreach((*vs)->data, buzzvstig_foreach_entry, &p);
}
/* Return */
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);
}
int BuzzLOG (buzzvm_t vm) {
LOG << "BUZZ: ";
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:
LOG << "[nil]";
break;
case BUZZTYPE_INT:
LOG << o->i.value;
break;
case BUZZTYPE_FLOAT:
LOG << o->f.value;
break;
case BUZZTYPE_TABLE:
LOG << "[table with " << (buzzdict_size(o->t.value)) << " elems]";
break;
case BUZZTYPE_CLOSURE:
if(o->c.value.isnative)
LOG << "[n-closure @" << o->c.value.ref << "]";
else
LOG << "[c-closure @" << o->c.value.ref << "]";
break;
case BUZZTYPE_STRING:
LOG << o->s.value.str;
break;
case BUZZTYPE_USERDATA:
LOG << "[userdata @" << o->u.value << "]";
break;
default:
break;
}
}
LOG << std::endl;
LOG.Flush();
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);
}
int print(buzzvm_t vm) {
for(int 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:
fprintf(stdout, "[nil]");
break;
case BUZZTYPE_INT:
fprintf(stdout, "%d", o->i.value);
break;
case BUZZTYPE_FLOAT:
fprintf(stdout, "%f", o->f.value);
break;
case BUZZTYPE_TABLE:
fprintf(stdout, "[table with %d elems]", (buzzdict_size(o->t.value)));
break;
case BUZZTYPE_CLOSURE:
if(o->c.value.isnative)
fprintf(stdout, "[n-closure @%d]", o->c.value.ref);
else
fprintf(stdout, "[c-closure @%d]", o->c.value.ref);
break;
case BUZZTYPE_STRING:
fprintf(stdout, "%s", o->s.value.str);
break;
case BUZZTYPE_USERDATA:
fprintf(stdout, "[userdata @%p]", o->u.value);
break;
default:
break;
}
}
fprintf(stdout, "\n");
return buzzvm_ret0(vm);
}
int buzzvm_vstig_put(buzzvm_t vm) {
buzzvm_lnum_assert(vm, 2);
/* Get vstig id */
buzzvm_lload(vm, 0);
buzzvm_pushs(vm, buzzvm_string_register(vm, "id"));
buzzvm_tget(vm);
uint16_t id = buzzvm_stack_at(vm, 1)->i.value;
/* Get key */
buzzvm_lload(vm, 1);
buzzobj_t k = buzzvm_stack_at(vm, 1);
/* Get value */
buzzvm_lload(vm, 2);
buzzobj_t v = buzzvm_stack_at(vm, 1);
/* Look for virtual stigmergy */
buzzvstig_t* vs = buzzdict_get(vm->vstigs, &id, buzzvstig_t);
if(vs) {
/* Look for the element */
buzzvstig_elem_t* x = buzzvstig_fetch(*vs, &k);
if(x) {
/* Element found, update it */
(*x)->data = v;
++((*x)->timestamp);
(*x)->robot = vm->robot;
/* Append a PUT message to the out message queue */
buzzoutmsg_queue_append_vstig(vm->outmsgs, BUZZMSG_VSTIG_PUT, id, k, *x);
}
else {
/* Element not found, create a new one */
buzzvstig_elem_t y = buzzvstig_elem_new(v, 1, vm->robot);
buzzvstig_store(*vs, &k, &y);
/* Append a PUT message to the out message queue */
buzzoutmsg_queue_append_vstig(vm->outmsgs, BUZZMSG_VSTIG_PUT, id, k, y);
}
}
/* Return */
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);
}
int buzzvm_swarm_leave(buzzvm_t vm) {
buzzvm_lnum_assert(vm, 0);
/* Get the id */
buzzvm_lload(vm, 0);
buzzvm_pushs(vm, buzzvm_string_register(vm, "id"));
buzzvm_tget(vm);
uint16_t id = buzzvm_stack_at(vm, 1)->i.value;
/* Leave the swarm, if known */
if(buzzdict_exists(vm->swarms, &id)) {
/* Store membership */
uint8_t v = 0;
buzzdict_set(vm->swarms, &id, &v);
/* Send update */
buzzoutmsg_queue_append_swarm_joinleave(
vm->outmsgs, BUZZMSG_SWARM_LEAVE, id);
/* Return */
return buzzvm_ret0(vm);
}
else {
vm->state = BUZZVM_STATE_ERROR;
vm->error = BUZZVM_ERROR_SWARM;
return BUZZVM_STATE_ERROR;
}
}
