Example #1
0
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);
}
Example #2
0
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;
}
Example #3
0
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);
}
Example #4
0
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;
}
Example #6
0
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);
}
Example #8
0
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);
}
Example #10
0
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);
}
Example #11
0
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;
}
Example #12
0
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);
}
Example #14
0
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;
}
Example #15
0
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);
}
Example #16
0
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);
}
Example #19
0
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);
}
Example #21
0
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;
}
Example #24
0
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);
      }
   }
}
Example #27
0
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;
   }
}
Example #28
0
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);
      }
   }
}
Example #29
0
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;
}
Example #30
0
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;
   }
}