Exemplo n.º 1
0
void CBuzzControllerFootBot::UpdateSensors() {
   /*
    * Update generic sensors
    */
   CBuzzController::UpdateSensors();
   /*
    * Update proximity sensor table
    */
   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_FootBotProximitySensor::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);
      }
   }
}
Exemplo n.º 2
0
buzzvm_state CBuzzController::Register(const std::string& str_key,
                                       const CVector3& c_vec) {
   buzzvm_pushs(m_tBuzzVM, buzzvm_string_register(m_tBuzzVM, str_key.c_str(), 1));
   buzzvm_pusht(m_tBuzzVM);
   buzzobj_t tVecTable = buzzvm_stack_at(m_tBuzzVM, 1);
   buzzvm_gstore(m_tBuzzVM);
   TablePut(tVecTable, "x", c_vec.GetX());
   TablePut(tVecTable, "y", c_vec.GetY());
   TablePut(tVecTable, "z", c_vec.GetZ());
   return m_tBuzzVM->state;
}
Exemplo n.º 3
0
buzzvm_state CBuzzController::Register(const std::string& str_key,
                                       const CColor& c_color) {
   buzzvm_pushs(m_tBuzzVM, buzzvm_string_register(m_tBuzzVM, str_key.c_str(), 1));
   buzzvm_pusht(m_tBuzzVM);
   buzzobj_t tColorTable = buzzvm_stack_at(m_tBuzzVM, 1);
   buzzvm_gstore(m_tBuzzVM);
   TablePut(tColorTable, "red", c_color.GetRed());
   TablePut(tColorTable, "green", c_color.GetGreen());
   TablePut(tColorTable, "blue", c_color.GetBlue());
   return m_tBuzzVM->state;
}
Exemplo n.º 4
0
buzzvm_state CBuzzController::Register(const std::string& str_key,
                                       const CQuaternion& c_quat) {
   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_gstore(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;
}
Exemplo n.º 5
0
buzzvm_state CBuzzController::TablePut(buzzobj_t t_table,
                                       SInt32 n_idx,
                                       const CColor& c_color) {
   buzzvm_push(m_tBuzzVM, t_table);
   buzzvm_pushi(m_tBuzzVM, n_idx);
   buzzvm_pusht(m_tBuzzVM);
   buzzobj_t tColorTable = buzzvm_stack_at(m_tBuzzVM, 1);
   buzzvm_tput(m_tBuzzVM);
   TablePut(tColorTable, "red", c_color.GetRed());
   TablePut(tColorTable, "green", c_color.GetGreen());
   TablePut(tColorTable, "blue", c_color.GetBlue());
   return m_tBuzzVM->state;   
}
Exemplo n.º 6
0
buzzvm_state CBuzzController::TablePut(buzzobj_t t_table,
                                       SInt32 n_idx,
                                       const CVector3& c_vec) {
   buzzvm_push(m_tBuzzVM, t_table);
   buzzvm_pushi(m_tBuzzVM, n_idx);
   buzzvm_pusht(m_tBuzzVM);
   buzzobj_t tVecTable = buzzvm_stack_at(m_tBuzzVM, 1);
   buzzvm_tput(m_tBuzzVM);
   TablePut(tVecTable, "x", c_vec.GetX());
   TablePut(tVecTable, "y", c_vec.GetY());
   TablePut(tVecTable, "z", c_vec.GetZ());
   return m_tBuzzVM->state;
}
Exemplo n.º 7
0
buzzvm_state CBuzzController::TablePut(buzzobj_t t_table,
                                       SInt32 n_idx,
                                       const CQuaternion& c_quat) {
   buzzvm_push(m_tBuzzVM, t_table);
   buzzvm_pushi(m_tBuzzVM, n_idx);
   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;
}
Exemplo n.º 8
0
void CBuzzController::UpdateSensors() {
   /*
    * Update positioning sensor
    */
   if(m_pcPos != NULL) {
      /* Get positioning readings */
      const CCI_PositioningSensor::SReading& sPosRead = m_pcPos->GetReading();
      /* Create empty positioning data table */
      buzzobj_t tPose = buzzheap_newobj(m_tBuzzVM, BUZZTYPE_TABLE);
      /* Store position data */
      TablePut(tPose, "position", sPosRead.Position);
      /* Store orientation data */
      TablePut(tPose, "orientation", sPosRead.Orientation);
      /* Register positioning data table as global symbol */
      Register("pose", tPose);
   }
}
Exemplo n.º 9
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);
      }
   }
}
Exemplo n.º 10
0
void CBuzzControllerFootBot::UpdateSensors() {
   /*
    * Update generic sensors
    */
   CBuzzController::UpdateSensors();
   /*
    * Update proximity sensor table
    */
   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_FootBotProximitySensor::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);
      }
   }
   /*
    * 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_ColoredBlobOmnidirectionalCameraSensor::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, "distance", sBlobs.BlobList[i]->Distance);
         TablePut(tEntry, "angle",    sBlobs.BlobList[i]->Angle);
         TablePut(tEntry, "color",    sBlobs.BlobList[i]->Color);
      }
   }
}
Exemplo n.º 11
0
buzzvm_state CBuzzController::TablePut(buzzobj_t t_table,
                                       const std::string& str_key,
                                       const CRadians& c_angle) {
   return TablePut(t_table, str_key, c_angle.GetValue());
}
Exemplo n.º 12
0
buzzvm_state CBuzzController::TablePut(buzzobj_t t_table,
                                       SInt32 n_idx,
                                       const CRadians& c_angle) {
   return TablePut(t_table, n_idx, c_angle.GetValue());
}