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; } }