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);
}
buzzvm_state CBuzzController::TablePut(buzzobj_t t_table,
const std::string& str_key,
const CQuaternion& c_quat) {
buzzvm_push(m_tBuzzVM, t_table);
buzzvm_pushs(m_tBuzzVM, buzzvm_string_register(m_tBuzzVM, str_key.c_str(), 1));
buzzvm_pusht(m_tBuzzVM);
buzzobj_t tQuatTable = buzzvm_stack_at(m_tBuzzVM, 1);
buzzvm_tput(m_tBuzzVM);
CRadians cYaw, cPitch, cRoll;
c_quat.ToEulerAngles(cYaw, cPitch, cRoll);
TablePut(tQuatTable, "yaw", cYaw);
TablePut(tQuatTable, "pitch", cPitch);
TablePut(tQuatTable, "roll", cRoll);
return m_tBuzzVM->state;
}
int BuzzDebugRayClear(buzzvm_t vm) {
/*
* Possible signatures
* debug.rays.clear()
* deletes all the rays
*/
/* 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().RayClear();
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 */
CVector3 cDir(buzzvm_stack_at(vm, 2)->f.value,
buzzvm_stack_at(vm, 1)->f.value,
0.0f);
/* 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)->SetDirection(cDir);
return buzzvm_ret0(vm);
}
buzzvm_state CBuzzControllerEFootBot::RegisterFunctions() {
/* Register base functions */
CBuzzController::RegisterFunctions();
Register("robot_type", 99);
if(m_pcWheels) {
/* BuzzSetWheelsFb */
buzzvm_pushs(m_tBuzzVM, buzzvm_string_register(m_tBuzzVM, "set_wheels", 1));
buzzvm_pushcc(m_tBuzzVM, buzzvm_function_register(m_tBuzzVM, BuzzSetWheelsFb));
buzzvm_gstore(m_tBuzzVM);
/* BuzzGoTo */
buzzvm_pushs(m_tBuzzVM, buzzvm_string_register(m_tBuzzVM, "goto", 1));
buzzvm_pushcc(m_tBuzzVM, buzzvm_function_register(m_tBuzzVM, BuzzGoTo));
buzzvm_gstore(m_tBuzzVM);
/* BuzzGet */
}
if(m_pcLEDs) {
/* BuzzSetLEDsFb */
buzzvm_pushs(m_tBuzzVM, buzzvm_string_register(m_tBuzzVM, "setleds", 1));
buzzvm_pushcc(m_tBuzzVM, buzzvm_function_register(m_tBuzzVM, BuzzSetLEDsFb));
buzzvm_gstore(m_tBuzzVM);
}
/* BuzzStartCharging */
if(m_batterySensor) {
buzzvm_pushs(m_tBuzzVM, buzzvm_string_register(m_tBuzzVM, "start_charging", 1));
buzzvm_pushcc(m_tBuzzVM, buzzvm_function_register(m_tBuzzVM, BuzzStartCharging));
buzzvm_gstore(m_tBuzzVM);
}
/* BuzzStopCharging */
if(m_batterySensor) {
buzzvm_pushs(m_tBuzzVM, buzzvm_string_register(m_tBuzzVM, "stop_charging", 1));
buzzvm_pushcc(m_tBuzzVM, buzzvm_function_register(m_tBuzzVM, BuzzStopCharging));
buzzvm_gstore(m_tBuzzVM);
}
/* BuzzStartProcessing */
if(m_batterySensor) {
buzzvm_pushs(m_tBuzzVM, buzzvm_string_register(m_tBuzzVM, "start_processing", 1));
buzzvm_pushcc(m_tBuzzVM, buzzvm_function_register(m_tBuzzVM, BuzzStartProcessing));
buzzvm_gstore(m_tBuzzVM);
}
/* BuzzStopProcessing */
if(m_batterySensor) {
buzzvm_pushs(m_tBuzzVM, buzzvm_string_register(m_tBuzzVM, "stop_processing", 1));
buzzvm_pushcc(m_tBuzzVM, buzzvm_function_register(m_tBuzzVM, BuzzStopProcessing));
buzzvm_gstore(m_tBuzzVM);
}
return m_tBuzzVM->state;
}
int buzzneighbors_reset(buzzvm_t vm) {
if(vm->state != BUZZVM_STATE_READY) return vm->state;
/* Make new table */
buzzobj_t t;
vm->state = make_table(vm, &t);
if(vm->state != BUZZVM_STATE_READY) return vm->state;
/* Add extra methods */
function_register(t, "broadcast", buzzneighbors_broadcast);
function_register(t, "listen", buzzneighbors_listen);
function_register(t, "ignore", buzzneighbors_ignore);
/* Register table as global symbol */
buzzvm_pushs(vm, buzzvm_string_register(vm, "neighbors", 1));
buzzvm_push(vm, t);
buzzvm_gstore(vm);
return vm->state;
}
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 main(int argc, char** argv) {
/* Parse command line */
if(argc != 2) {
fprintf(stderr, "Usage:\n\t%s <file.bo>\n\n", argv[0]);
return 0;
}
/* Open file */
int fd = open(argv[1], O_RDONLY);
if(fd < 0) perror(argv[1]);
/* Read data */
size_t bcode_size = lseek(fd, 0, SEEK_END);
lseek(fd, 0, SEEK_SET);
uint8_t* bcode_buf = (uint8_t*)malloc(bcode_size);
ssize_t rd;
size_t tot = 0;
while(tot < bcode_size) {
rd = read(fd, bcode_buf + tot, bcode_size - tot);
if(rd < 0) perror(argv[1]);
tot += rd;
}
close(fd);
/* Create new VM */
buzzvm_t vm = buzzvm_new(1);
/* Set byte code */
buzzvm_set_bcode(vm, bcode_buf, bcode_size);
/* Register hook function */
buzzvm_pushs(vm, buzzvm_string_register(vm, "hook"));
buzzvm_pushcc(vm, buzzvm_function_register(vm, hook));
buzzvm_gstore(vm);
/* Run byte code and dump state */
do dump(vm);
while(buzzvm_step(vm) == BUZZVM_STATE_READY);
if(vm->state == BUZZVM_STATE_DONE) {
dump(vm);
fprintf(stdout, "%s: execution terminated correctly\n\n",
argv[1]);
}
else {
fprintf(stderr, "%s: execution terminated abnormally: %s\n\n",
argv[1],
buzzvm_error_desc[vm->error]);
}
/* Destroy VM */
free(bcode_buf);
buzzvm_destroy(&vm);
return 0;
}
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);
}
static int make_table(buzzvm_t vm, uint16_t id) {
/* Create a table and add data and methods */
buzzobj_t t = buzzheap_newobj(vm->heap, BUZZTYPE_TABLE);
function_register(t, "others", buzzvm_swarm_others);
function_register(t, "join", buzzvm_swarm_join);
function_register(t, "leave", buzzvm_swarm_leave);
function_register(t, "in", buzzvm_swarm_in);
function_register(t, "select", buzzvm_swarm_select);
function_register(t, "exec", buzzvm_swarm_exec);
function_register(t, "others", buzzvm_swarm_others);
/* Register swarm id */
buzzvm_push(vm, t);
buzzvm_pushs(vm, buzzvm_string_register(vm, "id"));
buzzvm_pushi(vm, id);
buzzvm_tput(vm);
/* Push the table on the stack */
buzzvm_push(vm, t);
return BUZZVM_STATE_READY;
}
int buzzvm_swarm_in(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;
/* 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;
}
/* Push the return value */
buzzvm_pushi(vm, *x);
/* Return */
return buzzvm_ret1(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 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 buzzvm_vstig_size(buzzvm_t vm) {
buzzvm_lnum_assert(vm, 0);
/* 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, return its size */
buzzvm_pushi(vm, buzzdict_size((*vs)->data));
}
else {
/* Virtual stigmergy not found, return 0 */
buzzvm_pushi(vm, 0);
}
/* Return */
return buzzvm_ret1(vm);
}
int buzzvm_vstig_get(buzzvm_t 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;
/* Get key */
buzzvm_lload(vm, 1);
buzzobj_t k = buzzvm_stack_at(vm, 1);
/* Look for virtual stigmergy */
buzzvstig_t* vs = buzzdict_get(vm->vstigs, &id, buzzvstig_t);
if(vs) {
/* Virtual stigmergy found */
/* Look for key and push result */
buzzvstig_elem_t* e = buzzvstig_fetch(*vs, &k);
if(e) {
/* Key found */
buzzvm_push(vm, (*e)->data);
/* Append the message to the out message queue */
buzzoutmsg_queue_append_vstig(vm->outmsgs, BUZZMSG_VSTIG_QUERY, id, k, *e);
}
else {
/* Key not found, add a new one containing nil */
buzzvm_pushnil(vm);
buzzvstig_elem_t x =
buzzvstig_elem_new(buzzvm_stack_at(vm, 1),
1, vm->robot);
/* Append the message to the out message queue */
buzzoutmsg_queue_append_vstig(vm->outmsgs, BUZZMSG_VSTIG_QUERY, id, k, x);
}
}
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_ret1(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 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 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 buzzvm_vstig_create(buzzvm_t vm) {
buzzvm_lnum_assert(vm, 1);
/* Get vstig id */
buzzvm_lload(vm, 1);
buzzvm_type_assert(vm, 1, BUZZTYPE_INT);
uint16_t id = buzzvm_stack_at(vm, 1)->i.value;
buzzvm_pop(vm);
/* Look for virtual stigmergy */
buzzvstig_t* vs = buzzdict_get(vm->vstigs, &id, buzzvstig_t);
if(vs)
/* Found, destroy it */
buzzdict_remove(vm->vstigs, &id);
/* Create a new virtual stigmergy */
buzzvstig_t nvs = buzzvstig_new();
buzzdict_set(vm->vstigs, &id, &nvs);
/* Create a table and add data and methods */
buzzobj_t t = buzzheap_newobj(vm->heap, BUZZTYPE_TABLE);
buzzvm_push(vm, t);
buzzvm_pushs(vm, buzzvm_string_register(vm, "size"));
buzzvm_pushcc(vm, buzzvm_function_register(vm, buzzvm_vstig_size));
buzzvm_tput(vm);
buzzvm_push(vm, t);
buzzvm_pushs(vm, buzzvm_string_register(vm, "put"));
buzzvm_pushcc(vm, buzzvm_function_register(vm, buzzvm_vstig_put));
buzzvm_tput(vm);
buzzvm_push(vm, t);
buzzvm_pushs(vm, buzzvm_string_register(vm, "get"));
buzzvm_pushcc(vm, buzzvm_function_register(vm, buzzvm_vstig_get));
buzzvm_tput(vm);
buzzvm_push(vm, t);
buzzvm_pushs(vm, buzzvm_string_register(vm, "onconflict"));
buzzvm_pushcc(vm, buzzvm_function_register(vm, buzzvm_vstig_setonconflict));
buzzvm_tput(vm);
buzzvm_push(vm, t);
buzzvm_pushs(vm, buzzvm_string_register(vm, "onconflictlost"));
buzzvm_pushcc(vm, buzzvm_function_register(vm, buzzvm_vstig_setonconflictlost));
buzzvm_tput(vm);
buzzvm_push(vm, t);
buzzvm_pushs(vm, buzzvm_string_register(vm, "id"));
buzzvm_pushi(vm, id);
buzzvm_tput(vm);
/* Push the table on the stack */
buzzvm_push(vm, t);
/* Return */
return buzzvm_ret1(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);
}
buzzvm_state CBuzzControllerFootBot::RegisterFunctions() {
/* Register base functions */
CBuzzController::RegisterFunctions();
if(m_pcWheels) {
/* BuzzSetWheels */
buzzvm_pushs(m_tBuzzVM, buzzvm_string_register(m_tBuzzVM, "set_wheels", 1));
buzzvm_pushcc(m_tBuzzVM, buzzvm_function_register(m_tBuzzVM, BuzzSetWheels));
buzzvm_gstore(m_tBuzzVM);
/* BuzzGoTo with Cartesian coordinates */
buzzvm_pushs(m_tBuzzVM, buzzvm_string_register(m_tBuzzVM, "goto", 1));
buzzvm_pushcc(m_tBuzzVM, buzzvm_function_register(m_tBuzzVM, BuzzGoToC));
buzzvm_gstore(m_tBuzzVM);
buzzvm_pushs(m_tBuzzVM, buzzvm_string_register(m_tBuzzVM, "gotoc", 1));
buzzvm_pushcc(m_tBuzzVM, buzzvm_function_register(m_tBuzzVM, BuzzGoToC));
buzzvm_gstore(m_tBuzzVM);
/* BuzzGoTo with Polar coordinates */
buzzvm_pushs(m_tBuzzVM, buzzvm_string_register(m_tBuzzVM, "gotop", 1));
buzzvm_pushcc(m_tBuzzVM, buzzvm_function_register(m_tBuzzVM, BuzzGoToP));
buzzvm_gstore(m_tBuzzVM);
}
if(m_pcLEDs) {
/* BuzzSetLEDs */
buzzvm_pushs(m_tBuzzVM, buzzvm_string_register(m_tBuzzVM, "set_leds", 1));
buzzvm_pushcc(m_tBuzzVM, buzzvm_function_register(m_tBuzzVM, BuzzSetLEDs));
buzzvm_gstore(m_tBuzzVM);
}
if(m_pcCamera) {
/* BuzzCameraEnable */
buzzvm_pushs(m_tBuzzVM, buzzvm_string_register(m_tBuzzVM, "camera_enable", 1));
buzzvm_pushcc(m_tBuzzVM, buzzvm_function_register(m_tBuzzVM, BuzzCameraEnable));
buzzvm_gstore(m_tBuzzVM);
}
if(m_pcCamera) {
/* BuzzCameraDisable */
buzzvm_pushs(m_tBuzzVM, buzzvm_string_register(m_tBuzzVM, "camera_disable", 1));
buzzvm_pushcc(m_tBuzzVM, buzzvm_function_register(m_tBuzzVM, BuzzCameraDisable));
buzzvm_gstore(m_tBuzzVM);
}
return m_tBuzzVM->state;
}
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);
}
void CBuzzControllerEFootBot::UpdateSensors() {
/*
* Update generic sensors
*/
CBuzzController::UpdateSensors();
/*
* Update proximity sensor table
*/
if(m_batterySensor != NULL) {
Register("soc", m_batterySensor->GetSoc());
}
if(m_pcProximity != NULL) {
/* Create empty proximity table */
buzzvm_pushs(m_tBuzzVM, buzzvm_string_register(m_tBuzzVM, "proximity", 1));
buzzvm_pusht(m_tBuzzVM);
buzzobj_t tProxTable = buzzvm_stack_at(m_tBuzzVM, 1);
buzzvm_gstore(m_tBuzzVM);
/* Get proximity readings */
const CCI_EFootBotProximitySensor::TReadings& tProxReads = m_pcProximity->GetReadings();
/* Fill into the proximity table */
buzzobj_t tProxRead;
for(size_t i = 0; i < tProxReads.size(); ++i) {
/* Create table for i-th read */
buzzvm_pusht(m_tBuzzVM);
tProxRead = buzzvm_stack_at(m_tBuzzVM, 1);
buzzvm_pop(m_tBuzzVM);
/* Fill in the read */
TablePut(tProxRead, "value", tProxReads[i].Value);
TablePut(tProxRead, "angle", tProxReads[i].Angle);
/* Store read table in the proximity table */
TablePut(tProxTable, i, tProxRead);
}
}
}
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;
}
}
void CBuzzControllerSpiri::UpdateSensors() {
/* Positioning */
if(m_pcPosition) {
Register("position", m_pcPosition->GetReading().Position);
Register("orientation", m_pcPosition->GetReading().Orientation);
}
/* Camera */
if(m_pcCamera) {
buzzvm_pushs(m_tBuzzVM, buzzvm_string_register(m_tBuzzVM, "blobs", 1));
buzzvm_pusht(m_tBuzzVM);
buzzobj_t tBlobs = buzzvm_stack_at(m_tBuzzVM, 1);
buzzvm_gstore(m_tBuzzVM);
const CCI_ColoredBlobPerspectiveCameraSensor::SReadings& sBlobs = m_pcCamera->GetReadings();
for(size_t i = 0; i < sBlobs.BlobList.size(); ++i) {
buzzvm_pusht(m_tBuzzVM);
buzzobj_t tEntry = buzzvm_stack_at(m_tBuzzVM, 1);
buzzvm_pop(m_tBuzzVM);
TablePut(tBlobs, i, tEntry);
TablePut(tEntry, "px", sBlobs.BlobList[i]->X);
TablePut(tEntry, "py", sBlobs.BlobList[i]->Y);
TablePut(tEntry, "color", sBlobs.BlobList[i]->Color);
}
}
}
buzzvm_state CBuzzControllerSpiri::RegisterFunctions() {
/* Register base functions */
if(CBuzzController::RegisterFunctions() != BUZZVM_STATE_READY)
return m_tBuzzVM->state;
/* BuzzTakeOff */
if(m_pcPropellers && m_pcPosition) {
buzzvm_pushs(m_tBuzzVM, buzzvm_string_register(m_tBuzzVM, "takeoff", 1));
buzzvm_pushcc(m_tBuzzVM, buzzvm_function_register(m_tBuzzVM, BuzzTakeOff));
buzzvm_gstore(m_tBuzzVM);
}
/* BuzzLand */
if(m_pcPropellers && m_pcPosition) {
buzzvm_pushs(m_tBuzzVM, buzzvm_string_register(m_tBuzzVM, "land", 1));
buzzvm_pushcc(m_tBuzzVM, buzzvm_function_register(m_tBuzzVM, BuzzLand));
buzzvm_gstore(m_tBuzzVM);
}
/* BuzzGoTo */
if(m_pcPropellers) {
buzzvm_pushs(m_tBuzzVM, buzzvm_string_register(m_tBuzzVM, "goto", 1));
buzzvm_pushcc(m_tBuzzVM, buzzvm_function_register(m_tBuzzVM, BuzzGoTo));
buzzvm_gstore(m_tBuzzVM);
}
/* BuzzRotate */
if(m_pcPropellers) {
buzzvm_pushs(m_tBuzzVM, buzzvm_string_register(m_tBuzzVM, "rotate", 1));
buzzvm_pushcc(m_tBuzzVM, buzzvm_function_register(m_tBuzzVM, BuzzRotate));
buzzvm_gstore(m_tBuzzVM);
}
/* BuzzCameraEnable */
if(m_pcCamera) {
buzzvm_pushs(m_tBuzzVM, buzzvm_string_register(m_tBuzzVM, "camera_enable", 1));
buzzvm_pushcc(m_tBuzzVM, buzzvm_function_register(m_tBuzzVM, BuzzCameraEnable));
buzzvm_gstore(m_tBuzzVM);
}
/* BuzzCameraDisable */
if(m_pcCamera) {
buzzvm_pushs(m_tBuzzVM, buzzvm_string_register(m_tBuzzVM, "camera_disable", 1));
buzzvm_pushcc(m_tBuzzVM, buzzvm_function_register(m_tBuzzVM, BuzzCameraDisable));
buzzvm_gstore(m_tBuzzVM);
}
return m_tBuzzVM->state;
}
buzzvstig_elem_t buzzvstig_onconflict_call(buzzvm_t vm,
buzzvstig_t vs,
buzzobj_t k,
buzzvstig_elem_t lv,
buzzvstig_elem_t rv) {
/* Was a conflict manager defined? */
if(vs->onconflict) {
/* Push closure */
buzzvm_push(vm, vs->onconflict);
/* Push key */
buzzvm_push(vm, k);
/* Make table for local value */
buzzvm_pusht(vm);
add_field(robot, lv, pushi);
add_field(data, lv, push);
add_field(timestamp, lv, pushi);
/* Make table for remote value */
buzzvm_pusht(vm);
add_field(robot, rv, pushi);
add_field(data, rv, push);
add_field(timestamp, rv, pushi);
/* Call closure (key, lv, rv on the stack) */
buzzvm_closure_call(vm, 3);
/* Make new entry with return value */
/* Make sure it's a table */
if(buzzvm_stack_at(vm, 1)->o.type != BUZZTYPE_TABLE) {
fprintf(stderr, "[WARNING] [ROBOT %u] virtual stigmergy onconflict(): Return value type is %s, expected table\n", vm->robot, buzztype_desc[buzzvm_stack_at(vm, 1)->o.type]);
return NULL;
}
/* Get the robot id */
buzzobj_t ret = buzzvm_stack_at(vm, 1);
buzzvm_pushs(vm, buzzvm_string_register(vm, "robot", 1));
buzzvm_tget(vm);
if(buzzvm_stack_at(vm, 1)->o.type != BUZZTYPE_INT)
return NULL;
uint16_t robot = buzzvm_stack_at(vm, 1)->i.value;
buzzvm_pop(vm);
/* Get the data */
buzzvm_push(vm, ret);
buzzvm_pushs(vm, buzzvm_string_register(vm, "data", 1));
buzzvm_tget(vm);
buzzobj_t data = buzzvm_stack_at(vm, 1);
buzzvm_pop(vm);
/* Make new entry */
return buzzvstig_elem_new(data, lv->timestamp, robot);
}
else {
/* No conflict manager, use default behavior */
/* If both values are not nil, keep the value of the robot with the higher id */
if(((lv->data->o.type == BUZZTYPE_NIL) && (rv->data->o.type == BUZZTYPE_NIL)) ||
((lv->data->o.type != BUZZTYPE_NIL) && (rv->data->o.type != BUZZTYPE_NIL))) {
if(lv->robot > rv->robot)
return buzzvstig_elem_clone(vm, lv);
else
return buzzvstig_elem_clone(vm, rv);
}
/* If my value is not nil, keep mine */
if(lv->data->o.type != BUZZTYPE_NIL)
return buzzvstig_elem_clone(vm, lv);
/* If the other value is not nil, keep that one */
if(rv->data->o.type != BUZZTYPE_NIL)
return buzzvstig_elem_clone(vm, rv);
/* Otherwise nothing to do */
return NULL;
}
}
