static int BuzzSetWheels(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<CBuzzControllerFootBot*>( 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 buzzneighbors_kin(buzzvm_t vm) { buzzvm_lnum_assert(vm, 0); /* Initialize the swarm id to 'unknown' */ int32_t swarmid = -1; /* If the swarm stack is not empty, look for the swarm id */ if(!buzzdarray_isempty(vm->swarmstack)) { /* Get position in swarm stack */ uint16_t sstackpos = 1; if(buzzdarray_size(vm->lsyms->syms) > 1) sstackpos = buzzdarray_get(vm->lsyms->syms, 1, buzzobj_t)->i.value; /* Get swarm id */ if(sstackpos <= buzzdarray_size(vm->swarmstack)) swarmid = buzzdarray_get(vm->swarmstack, buzzdarray_size(vm->swarmstack) - sstackpos, uint16_t); } /* Get the self table */ buzzvm_lload(vm, 0); buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE); /* Get the data table */ buzzvm_pushs(vm, buzzvm_string_register(vm, "data", 1)); buzzvm_tget(vm); buzzobj_t data = buzzvm_stack_at(vm, 1); /* Create a new table as return value */ buzzobj_t t; vm->state = make_table(vm, &t); if(vm->state != BUZZVM_STATE_READY) return vm->state; /* If data is available, filter it */ if(data->o.type == BUZZTYPE_TABLE) { /* Create a new data table */ buzzobj_t kindata = buzzheap_newobj(vm->heap, BUZZTYPE_TABLE); /* Filter the neighbors in data and add them to kindata */ struct neighbor_filter_s fdata = { .vm = vm, .swarm_id = swarmid, .result = kindata->t.value }; buzzdict_foreach(data->t.value, neighbor_filter_kin, &fdata); /* Add kindata as the "data" field in t */ buzzvm_push(vm, t); buzzvm_pushs(vm, buzzvm_string_register(vm, "data", 1)); buzzvm_push(vm, kindata); buzzvm_tput(vm); } /* Return the table */ buzzvm_push(vm, t); return buzzvm_ret1(vm); }
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 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 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 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 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 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 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 buzzvstig_get(buzzvm_t vm) { buzzvm_lnum_assert(vm, 1); /* Get vstig id */ id_get(); /* Get key */ buzzvm_lload(vm, 1); buzzobj_t k = buzzvm_stack_at(vm, 1); /* Look for virtual stigmergy */ const buzzvstig_t* vs = buzzdict_get(vm->vstigs, &id, buzzvstig_t); if(vs) { /* Virtual stigmergy found */ /* Look for key and push result */ const 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, BUZZMSG_VSTIG_QUERY, id, k, *e); } else { /* Key not found, make a new one containing nil */ buzzvm_pushnil(vm); buzzvstig_elem_t x = buzzvstig_elem_new(buzzvm_stack_at(vm, 1), // nil value 0, // timestamp vm->robot); // robot id /* Append the message to the out message queue */ buzzoutmsg_queue_append_vstig(vm, BUZZMSG_VSTIG_QUERY, id, k, x); free(x); } } else { /* No virtual stigmergy found, just push false */ /* If this happens, its a bug */ buzzvm_pushnil(vm); } /* Return the value found */ return buzzvm_ret1(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_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); }
int buzzvstig_onconflictlost(struct buzzvm_s* vm) { 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); /* Clone the closure */ if((*vs)->onconflictlost) free((*vs)->onconflictlost); (*vs)->onconflictlost = buzzheap_clone(vm, 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); }
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; } }