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);
}
}
}
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;
}
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;
}
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;
}
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;
}
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;
}
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;
}
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);
}
}
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);
}
}
}
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);
}
}
}
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());
}
buzzvm_state CBuzzController::TablePut(buzzobj_t t_table,
SInt32 n_idx,
const CRadians& c_angle) {
return TablePut(t_table, n_idx, c_angle.GetValue());
}
