void CFootBotCircle::ControlStep() {
/* Send counter value */
CByteArray cBuf(10);
cBuf[0] = m_unCounter & 0xff;
cBuf[1] = m_unCounter >> 8 & 0xff;
cBuf[2] = m_unCounter >> 16 & 0xff;
cBuf[3] = m_unCounter >> 24 & 0xff;
m_pcRABAct->SetData(cBuf);
/* Write on robot log the sent value */
RLOG << "Sent: " << m_unCounter << std::endl;
/* Increase counter */
++m_unCounter;
}
/*
* The stack can have one or two values
* - The case of one value is when you set an entire array
* - In the case of two values, you must have:
* 1. the idx of the data item to set
* 2. the value of the data item in the range [0,255]
*/
int LuaRABSetData(lua_State* pt_lua_state) {
if(lua_gettop(pt_lua_state) == 1) {
/* Check parameters */
luaL_checktype(pt_lua_state, 1, LUA_TTABLE);
/* Get reference to actuator */
CCI_RangeAndBearingActuator* pcAct = CLuaUtility::GetDeviceInstance<CCI_RangeAndBearingActuator>(pt_lua_state, "range_and_bearing");
/* Check whether sizes match */
if(pcAct->GetSize() != lua_rawlen(pt_lua_state, -1)) {
return luaL_error(pt_lua_state, "robot.range_and_bearing.set_data(array) expects an array of %d numbers", pcAct->GetSize());
}
/* Fill up a byte array, checking that all elements are numbers */
CByteArray cBuf(pcAct->GetSize());
for(size_t i = 0; i < pcAct->GetSize(); ++i) {
lua_pushnumber(pt_lua_state, i+1);
lua_gettable(pt_lua_state, -2);
if(lua_type(pt_lua_state, -1) == LUA_TNUMBER) {
cBuf[i] = static_cast<UInt8>(lua_tonumber(pt_lua_state, -1));
lua_pop(pt_lua_state, 1);
}
else {
return luaL_error(pt_lua_state, "element #%d of the array is not a number", i+1);
}
}
/* Perform action */
pcAct->SetData(cBuf);
return 0;
}
else if(lua_gettop(pt_lua_state) == 2) {
/* Check parameters */
luaL_checktype(pt_lua_state, 1, LUA_TNUMBER);
luaL_checktype(pt_lua_state, 2, LUA_TNUMBER);
/* Get reference to actuator */
CCI_RangeAndBearingActuator* pcAct = CLuaUtility::GetDeviceInstance<CCI_RangeAndBearingActuator>(pt_lua_state, "range_and_bearing");
/* Check parameter values */
size_t unIdx = lua_tonumber(pt_lua_state, 1);
UInt8 unData = lua_tonumber(pt_lua_state, 2);
if(unIdx < 1 || unIdx > pcAct->GetSize()) {
return luaL_error(pt_lua_state, "passed index %d out of bounds [1,%d]", unIdx, pcAct->GetSize());
}
/* Perform action */
pcAct->SetData(unIdx-1, unData);
return 0;
}
else {
return luaL_error(pt_lua_state, "robot.range_and_bearing.set_data() expects either one or two arguments");
}
}
