void CBuzzController::ControlStep() {
/* Update debugging information */
m_sDebug.Clear();
if(m_sDebug.Trajectory.Tracking) {
const CCI_PositioningSensor::SReading& sPosRead = m_pcPos->GetReading();
m_sDebug.TrajectoryAdd(sPosRead.Position);
}
/* Take care of the rest */
if(m_tBuzzVM && m_tBuzzVM->state == BUZZVM_STATE_READY) {
ProcessInMsgs();
UpdateSensors();
if(buzzvm_function_call(m_tBuzzVM, "step", 0) != BUZZVM_STATE_READY) {
fprintf(stderr, "[ROBOT %u] %s: execution terminated abnormally: %s\n\n",
m_tBuzzVM->robot,
m_strBytecodeFName.c_str(),
ErrorInfo().c_str());
for(UInt32 i = 1; i <= buzzdarray_size(m_tBuzzVM->stacks); ++i) {
buzzdebug_stack_dump(m_tBuzzVM, i, stdout);
}
return;
}
ProcessOutMsgs();
}
else {
fprintf(stderr, "[ROBOT %s] Robot is not ready to execute Buzz script.\n\n",
GetId().c_str());
}
}
void CBuzzController::Destroy() {
/* Get rid of the VM */
if(m_tBuzzVM) {
buzzvm_function_call(m_tBuzzVM, "destroy", 0);
buzzvm_destroy(&m_tBuzzVM);
}
}
void CBuzzController::SetBytecode(const std::string& str_fname) {
/* Reset the BuzzVM */
if(m_tBuzzVM) buzzvm_destroy(&m_tBuzzVM);
m_tBuzzVM = buzzvm_new(m_unRobotId);
/* Save the bytecode filename */
m_strBytecodeFName = str_fname;
/* Load the bytecode */
std::ifstream cBCodeFile(str_fname.c_str(), std::ios::binary | std::ios::ate);
if(cBCodeFile.fail()) {
THROW_ARGOSEXCEPTION("Can't open file \"" << str_fname << "\": " << strerror(errno));
}
std::ifstream::pos_type unFileSize = cBCodeFile.tellg();
m_cBytecode.Clear();
m_cBytecode.Resize(unFileSize);
cBCodeFile.seekg(0, std::ios::beg);
cBCodeFile.read(reinterpret_cast<char*>(m_cBytecode.ToCArray()), unFileSize);
/* Load the script */
buzzvm_set_bcode(m_tBuzzVM, m_cBytecode.ToCArray(), m_cBytecode.Size());
if(buzzvm_set_bcode(m_tBuzzVM, m_cBytecode.ToCArray(), m_cBytecode.Size()) != BUZZVM_STATE_READY) {
THROW_ARGOSEXCEPTION("Error loading Buzz script \"" << str_fname << "\": " << buzzvm_strerror(m_tBuzzVM));
}
/* Register basic function */
if(RegisterFunctions() != BUZZVM_STATE_READY) {
THROW_ARGOSEXCEPTION("Error while registering functions");
}
/* Execute the global part of the script */
buzzvm_execute_script(m_tBuzzVM);
/* Call the Init() function */
buzzvm_function_call(m_tBuzzVM, "init", 0);
}
void CBuzzController::ControlStep() {
ProcessInMsgs();
UpdateSensors();
if(buzzvm_function_call(m_tBuzzVM, "step", 0) != BUZZVM_STATE_READY) {
fprintf(stderr, "[ROBOT %u] %s: execution terminated abnormally: %s\n\n",
m_tBuzzVM->robot,
m_strBytecodeFName.c_str(),
buzzvm_strerror(m_tBuzzVM));
buzzvm_dump(m_tBuzzVM);
}
ProcessOutMsgs();
}
