void Session::receiveWithoutFlush(PacketReader& packet) {
if(died)
return;
if (!dumpJustInDebug || (dumpJustInDebug && Logs::GetLevel()>=7))
DUMP(packet.data(),packet.size(),"Request from ",peer.address.toString())
packetHandler(packet);
}
/**
* Host MCU Interface Rx task
* @param param initialData
*/
static void HostInterface_RxTask( task_param_t param )
{
hostInterface_packet_t
tmpPkt;
while (1)
{
osa_status_t
status = HostInterface_RxQueueMsgGet( &tmpPkt );
if ( kStatus_OSA_Success == status )
{
#ifdef gHostInterface_TxConfirmationEnable
// acknowledge the packet reception
if ( 1 == ( tmpPkt.start2 & 0x01 ) )
{
HostInterface_EventSendOkPacketSet();
}
#endif
// handle the packet's content
packetHandler(&tmpPkt);
}
}
}
void Session::receive(PacketReader& packet) {
AESEngine aes = decoder();
if(!RTMFP::Decode(aes,packet)) {
ERROR("Decrypt error on session %u",id);
return;
}
Logs::Dump(packet,format("Request from %s",peer.address.toString()).c_str(),middleDump);
packetHandler(packet);
}
AREXPORT bool ArPTZ::robotPacketHandler(ArRobotPacket *packet)
{
//printf("%x\n", packet->getID());
if ((packet->getID() == 0xb0 && myAuxPort == 1) ||
(packet->getID() == 0xb8 && myAuxPort == 2) ||
(packet->getID() == 200 && myAuxPort == 3)
)
return packetHandler(packet);
else
return false;
}
bool Mineserver::run()
{
uint32_t starttime = (uint32_t)time(0);
uint32_t tick = (uint32_t)time(0);
// load plugins
if (config()->has("system.plugins") && (config()->type("system.plugins") == CONFIG_NODE_LIST))
{
std::list<std::string> tmp = config()->mData("system.plugins")->keys();
for (std::list<std::string>::const_iterator it = tmp.begin(); it != tmp.end(); ++it)
{
std::string path = config()->sData("system.path.plugins");
std::string name = config()->sData("system.plugins." + (*it));
std::string alias = *it;
if (name[0] == '_')
{
path = "";
alias = name;
name = name.substr(1);
}
plugin()->loadPlugin(name, path, alias);
}
}
// Initialize map
for (int i = 0; i < (int)m_map.size(); i++)
{
physics(i)->enabled = (config()->bData("system.physics.enabled"));
m_map[i]->init(i);
if (config()->bData("map.generate_spawn.enabled"))
{
LOG2(INFO, "Generating spawn area...");
int size = config()->iData("map.generate_spawn.size");
bool show_progress = config()->bData("map.generate_spawn.show_progress");
#ifdef __FreeBSD__
show_progress = false;
#endif
#ifdef WIN32
DWORD t_begin = 0, t_end = 0;
#else
clock_t t_begin = 0, t_end = 0;
#endif
for (int x = -size; x <= size; x++)
{
if (show_progress)
{
#ifdef WIN32
t_begin = timeGetTime();
#else
t_begin = clock();
#endif
}
for (int z = -size; z <= size; z++)
{
m_map[i]->loadMap(x, z);
}
if (show_progress)
{
#ifdef WIN32
t_end = timeGetTime();
LOG2(INFO, dtos((x + size + 1) *(size * 2 + 1)) + "/" + dtos((size * 2 + 1) *(size * 2 + 1)) + " done. " + dtos((t_end - t_begin) / (size * 2 + 1)) + "ms per chunk");
#else
t_end = clock();
LOG2(INFO, dtos((x + size + 1) *(size * 2 + 1)) + "/" + dtos((size * 2 + 1) *(size * 2 + 1)) + " done. " + dtos(((t_end - t_begin) / (CLOCKS_PER_SEC / 1000)) / (size * 2 + 1)) + "ms per chunk");
#endif
}
}
}
#ifdef DEBUG
LOG(DEBUG, "Map", "Spawn area ready!");
#endif
}
// Initialize packethandler
packetHandler()->init();
// Load ip from config
const std::string ip = config()->sData("net.ip");
// Load port from config
const int port = config()->iData("net.port");
#ifdef WIN32
WSADATA wsaData;
int iResult;
// Initialize Winsock
iResult = WSAStartup(MAKEWORD(2, 2), &wsaData);
if (iResult != 0)
{
LOG2(ERROR, std::string("WSAStartup failed with error: " + iResult));
return false;
}
#endif
struct sockaddr_in addresslisten;
int reuse = 1;
m_eventBase = reinterpret_cast<event_base*>(event_init());
#ifdef WIN32
m_socketlisten = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
#else
m_socketlisten = socket(AF_INET, SOCK_STREAM, 0);
#endif
if (m_socketlisten < 0)
{
LOG2(ERROR, "Failed to create listen socket");
return false;
}
memset(&addresslisten, 0, sizeof(addresslisten));
addresslisten.sin_family = AF_INET;
addresslisten.sin_addr.s_addr = inet_addr(ip.c_str());
addresslisten.sin_port = htons(port);
setsockopt(m_socketlisten, SOL_SOCKET, SO_REUSEADDR, (char*)&reuse, sizeof(reuse));
// Bind to port
if (bind(m_socketlisten, (struct sockaddr*)&addresslisten, sizeof(addresslisten)) < 0)
{
LOG2(ERROR, "Failed to bind to " + ip + ":" + dtos(port));
return false;
}
if (listen(m_socketlisten, 5) < 0)
{
LOG2(ERROR, "Failed to listen to socket");
return false;
}
setnonblock(m_socketlisten);
event_set(&m_listenEvent, m_socketlisten, EV_WRITE | EV_READ | EV_PERSIST, accept_callback, NULL);
event_add(&m_listenEvent, NULL);
LOG2(INFO, "Listening on: ");
if (ip == "0.0.0.0")
{
// Print all local IPs
char name[255];
gethostname(name, sizeof(name));
struct hostent* hostinfo = gethostbyname(name);
int ipIndex = 0;
while (hostinfo && hostinfo->h_addr_list[ipIndex])
{
const std::string ip(inet_ntoa(*(struct in_addr*)hostinfo->h_addr_list[ipIndex++]));
LOG2(INFO, ip + ":" + dtos(port));
}
}
else
{
LOG2(INFO, ip + ":" + dtos(port));
}
timeval loopTime;
loopTime.tv_sec = 0;
loopTime.tv_usec = 200000; // 200ms
m_running = true;
event_base_loopexit(m_eventBase, &loopTime);
// Create our Server Console user so we can issue commands
time_t timeNow = time(NULL);
while (m_running && event_base_loop(m_eventBase, 0) == 0)
{
event_base_loopexit(m_eventBase, &loopTime);
// Run 200ms timer hook
static_cast<Hook0<bool>*>(plugin()->getHook("Timer200"))->doAll();
// Alert any block types that care about timers
for (size_t i = 0 ; i < plugin()->getBlockCB().size(); ++i)
{
const BlockBasicPtr blockcb = plugin()->getBlockCB()[i];
if (blockcb != NULL)
{
blockcb->timer200();
}
}
//Update physics every 200ms
for (std::vector<Map*>::size_type i = 0 ; i < m_map.size(); i++)
{
physics(i)->update();
}
//Every 10 seconds..
timeNow = time(0);
if (timeNow - starttime > 10)
{
starttime = (uint32_t)timeNow;
//Map saving on configurable interval
if (m_saveInterval != 0 && timeNow - m_lastSave >= m_saveInterval)
{
//Save
for (std::vector<Map*>::size_type i = 0; i < m_map.size(); i++)
{
m_map[i]->saveWholeMap();
}
m_lastSave = timeNow;
}
// If users, ping them
if (!User::all().empty())
{
// Send server time
Packet pkt;
pkt << (int8_t)PACKET_TIME_UPDATE << (int64_t)m_map[0]->mapTime;
(*User::all().begin())->sendAll(pkt);
}
//Check for tree generation from saplings
for (size_t i = 0; i < m_map.size(); ++i)
{
m_map[i]->checkGenTrees();
}
// TODO: Run garbage collection for chunk storage dealie?
// Run 10s timer hook
static_cast<Hook0<bool>*>(plugin()->getHook("Timer10000"))->doAll();
}
// Every second
if (timeNow - tick > 0)
{
tick = (uint32_t)timeNow;
// Loop users
for (std::set<User*>::const_iterator it = users().begin(); it != users().end(); ++it)
{
// No data received in 30s, timeout
if ((*it)->logged && timeNow - (*it)->lastData > 30)
{
LOG2(INFO, "Player " + (*it)->nick + " timed out");
delete *it;
}
else if (!(*it)->logged && timeNow - (*it)->lastData > 100)
{
delete (*it);
}
else
{
if (m_damage_enabled)
{
(*it)->checkEnvironmentDamage();
}
(*it)->pushMap();
(*it)->popMap();
}
}
for (std::vector<Map*>::size_type i = 0 ; i < m_map.size(); i++)
{
m_map[i]->mapTime += 20;
if (m_map[i]->mapTime >= 24000)
{
m_map[i]->mapTime = 0;
}
}
for (std::set<User*>::const_iterator it = users().begin(); it != users().end(); ++it)
{
(*it)->pushMap();
(*it)->popMap();
}
// Check for Furnace activity
furnaceManager()->update();
// Run 1s timer hook
static_cast<Hook0<bool>*>(plugin()->getHook("Timer1000"))->doAll();
}
// Underwater check / drowning
// ToDo: this could be done a bit differently? - Fador
// -- User::all() == users() - louisdx
for (std::set<User*>::const_iterator it = users().begin(); it != users().end(); ++it)
{
(*it)->isUnderwater();
if ((*it)->pos.y < 0)
{
(*it)->sethealth((*it)->health - 5);
}
}
}
#ifdef WIN32
closesocket(m_socketlisten);
#else
close(m_socketlisten);
#endif
saveAll();
event_base_free(m_eventBase);
return true;
}
int main()
{
// Initialize PortHandler Structs
// Set the port path
// Get methods and members of PortHandlerLinux or PortHandlerWindows
int port_num = portHandler(DEVICENAME);
// Initialize PacketHandler Structs
packetHandler();
// Initialize Groupbulkread Structs
int group_num = groupBulkRead(port_num, PROTOCOL_VERSION);
int index = 0;
int dxl_comm_result = COMM_TX_FAIL; // Communication result
uint8_t dxl_addparam_result = False; // AddParam result
uint8_t dxl_getdata_result = False; // GetParam result
int dxl_goal_position[2] = { DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE }; // Goal position
uint8_t dxl_error = 0; // Dynamixel error
uint16_t dxl1_present_position = 0; // Present position
uint8_t dxl2_moving = 0; // Dynamixel moving status
// Open port
if (openPort(port_num))
{
printf("Succeeded to open the port!\n");
}
else
{
printf("Failed to open the port!\n");
printf("Press any key to terminate...\n");
getch();
return 0;
}
// Set port baudrate
if (setBaudRate(port_num, BAUDRATE))
{
printf("Succeeded to change the baudrate!\n");
}
else
{
printf("Failed to change the baudrate!\n");
printf("Press any key to terminate...\n");
getch();
return 0;
}
// Enable Dynamixel#1 Torque
write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL1_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE);
if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
{
printTxRxResult(PROTOCOL_VERSION, dxl_comm_result);
}
else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
{
printRxPacketError(PROTOCOL_VERSION, dxl_error);
}
else
{
printf("Dynamixel#%d has been successfully connected \n", DXL1_ID);
}
// Enable Dynamixel#2 Torque
write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL2_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE);
if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
{
printTxRxResult(PROTOCOL_VERSION, dxl_comm_result);
}
else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
{
printRxPacketError(PROTOCOL_VERSION, dxl_error);
}
else
{
printf("Dynamixel#%d has been successfully connected \n", DXL2_ID);
}
// Add parameter storage for Dynamixel#1 present position value
dxl_addparam_result = groupBulkReadAddParam(group_num, DXL1_ID, ADDR_MX_PRESENT_POSITION, LEN_MX_PRESENT_POSITION);
if (dxl_addparam_result != True)
{
fprintf(stderr, "[ID:%03d] groupBulkRead addparam failed", DXL1_ID);
return 0;
}
// Add parameter storage for Dynamixel#2 present moving value
dxl_addparam_result = groupBulkReadAddParam(group_num, DXL2_ID, ADDR_MX_MOVING, LEN_MX_MOVING);
if (dxl_addparam_result != True)
{
fprintf(stderr, "[ID:%03d] groupBulkRead addparam failed", DXL2_ID);
return 0;
}
while (1)
{
printf("Press any key to continue! (or press ESC to quit!)\n");
if (getch() == ESC_ASCII_VALUE)
break;
// Write Dynamixel#1 goal position
write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL1_ID, ADDR_MX_GOAL_POSITION, dxl_goal_position[index]);
if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
{
printTxRxResult(PROTOCOL_VERSION, dxl_comm_result);
}
else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
{
printRxPacketError(PROTOCOL_VERSION, dxl_error);
}
// Write Dynamixel#2 goal position
write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL2_ID, ADDR_MX_GOAL_POSITION, dxl_goal_position[index]);
if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
{
printTxRxResult(PROTOCOL_VERSION, dxl_comm_result);
}
else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
{
printRxPacketError(PROTOCOL_VERSION, dxl_error);
}
do
{
// Bulkread present position and moving status
groupBulkReadTxRxPacket(group_num);
if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
printTxRxResult(PROTOCOL_VERSION, dxl_comm_result);
dxl_getdata_result = groupBulkReadIsAvailable(group_num, DXL1_ID, ADDR_MX_PRESENT_POSITION, LEN_MX_PRESENT_POSITION);
if (dxl_getdata_result != True)
{
fprintf(stderr, "[ID:%03d] groupBulkRead getdata failed", DXL1_ID);
return 0;
}
dxl_getdata_result = groupBulkReadIsAvailable(group_num, DXL2_ID, ADDR_MX_MOVING, LEN_MX_MOVING);
if (dxl_getdata_result != True)
{
fprintf(stderr, "[ID:%03d] groupBulkRead getdata failed", DXL2_ID);
return 0;
}
// Get Dynamixel#1 present position value
dxl1_present_position = groupBulkReadGetData(group_num, DXL1_ID, ADDR_MX_PRESENT_POSITION, LEN_MX_PRESENT_POSITION);
// Get Dynamixel#2 moving status value
dxl2_moving = groupBulkReadGetData(group_num, DXL2_ID, ADDR_MX_MOVING, LEN_MX_MOVING);
printf("[ID:%03d] Present Position : %d \t [ID:%03d] Is Moving : %d\n", DXL1_ID, dxl1_present_position, DXL2_ID, dxl2_moving);
} while (abs(dxl_goal_position[index] - dxl1_present_position) > DXL_MOVING_STATUS_THRESHOLD);
// Change goal position
if (index == 0)
{
index = 1;
}
else
{
index = 0;
}
}
// Disable Dynamixel#1 Torque
write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL1_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_DISABLE);
if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
{
printTxRxResult(PROTOCOL_VERSION, dxl_comm_result);
}
else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
{
printRxPacketError(PROTOCOL_VERSION, dxl_error);
}
// Disable Dynamixel#2 Torque
write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL2_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_DISABLE);
if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
{
printTxRxResult(PROTOCOL_VERSION, dxl_comm_result);
}
else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
{
printRxPacketError(PROTOCOL_VERSION, dxl_error);
}
// Close port
closePort(port_num);
return 0;
}
int main()
{
// Initialize PortHandler Structs
// Set the port path
// Get methods and members of PortHandlerLinux or PortHandlerWindows
int port_num = portHandler(DEVICENAME);
// Initialize PacketHandler Structs
packetHandler();
int dxl_comm_result = COMM_TX_FAIL; // Communication result
uint8_t dxl_error = 0; // Dynamixel error
uint16_t dxl_model_number; // Dynamixel model number
// Open port
if (openPort(port_num))
{
printf("Succeeded to open the port!\n");
}
else
{
printf("Failed to open the port!\n");
printf("Press any key to terminate...\n");
getch();
return 0;
}
// Set port baudrate
if (setBaudRate(port_num, BAUDRATE))
{
printf("Succeeded to change the baudrate!\n");
}
else
{
printf("Failed to change the baudrate!\n");
printf("Press any key to terminate...\n");
getch();
return 0;
}
// Try to ping the Dynamixel
// Get Dynamixel model number
dxl_model_number = pingGetModelNum(port_num, PROTOCOL_VERSION, DXL_ID);
if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
{
printf("%s\n", getTxRxResult(PROTOCOL_VERSION, dxl_comm_result));
}
else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
{
printf("%s\n", getRxPacketError(PROTOCOL_VERSION, dxl_error));
}
else
{
printf("[ID:%03d] ping Succeeded. Dynamixel model number : %d\n", DXL_ID, dxl_model_number);
}
// Close port
closePort(port_num);
return 0;
}
int main()
{
// Initialize PortHandler Structs
// Set the port path
// Get methods and members of PortHandlerLinux or PortHandlerWindows
int port_num = portHandler(DEVICENAME);
// Initialize PacketHandler Structs
packetHandler();
int index = 0;
int dxl_comm_result = COMM_TX_FAIL; // Communication result
int dxl_goal_position[2] = { DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE }; // Goal position
uint8_t dxl_error = 0; // Dynamixel error
uint16_t dxl_present_position = 0; // Present position
// Open port
if (openPort(port_num))
{
printf("Succeeded to open the port!\n");
}
else
{
printf("Failed to open the port!\n");
printf("Press any key to terminate...\n");
getch();
return 0;
}
// Set port baudrate
if (setBaudRate(port_num, BAUDRATE))
{
printf("Succeeded to change the baudrate!\n");
}
else
{
printf("Failed to change the baudrate!\n");
printf("Press any key to terminate...\n");
getch();
return 0;
}
// Enable Dynamixel Torque
write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE);
if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
{
printTxRxResult(PROTOCOL_VERSION, dxl_comm_result);
}
else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
{
printRxPacketError(PROTOCOL_VERSION, dxl_error);
}
else
{
printf("Dynamixel has been successfully connected \n");
}
while (1)
{
printf("Press any key to continue! (or press ESC to quit!)\n");
if (getch() == ESC_ASCII_VALUE)
break;
// Write goal position
write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_GOAL_POSITION, dxl_goal_position[index]);
if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
{
printTxRxResult(PROTOCOL_VERSION, dxl_comm_result);
}
else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
{
printRxPacketError(PROTOCOL_VERSION, dxl_error);
}
do
{
// Read present position
dxl_present_position = read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_PRESENT_POSITION);
if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
{
printTxRxResult(PROTOCOL_VERSION, dxl_comm_result);
}
else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
{
printRxPacketError(PROTOCOL_VERSION, dxl_error);
}
printf("[ID:%03d] GoalPos:%03d PresPos:%03d\n", DXL_ID, dxl_goal_position[index], dxl_present_position);
} while ((abs(dxl_goal_position[index] - dxl_present_position) > DXL_MOVING_STATUS_THRESHOLD));
// Change goal position
if (index == 0)
{
index = 1;
}
else
{
index = 0;
}
}
// Disable Dynamixel Torque
write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_DISABLE);
if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
{
printTxRxResult(PROTOCOL_VERSION, dxl_comm_result);
}
else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
{
printRxPacketError(PROTOCOL_VERSION, dxl_error);
}
// Close port
closePort(port_num);
return 0;
}
void GrupPacket::startGruping( void ){
#ifdef DEBUG
std::cout<<PRINT<<"Reading and processing packet from shared Buffer\n";
#endif
int readCount;
readCount = 0;
Buffer *readBufferObj;
__rawNetData__ *packet;
bool flag;
flag = false;
try{
readBufferObj = new Buffer();
packet = new __rawNetData__();
}
catch ( const std::bad_alloc &ba ){
std::cerr<<PRINT<<"Exception:"<<ba.what()<<std::endl;
}
int readReturn;
while ( !flag ){
readReturn = readBufferObj->readBuffer( 1 , packet ) ;
std::cout<<PRINT<<"read Return:[ "<<readReturn<<" ] \n";
switch ( readReturn ){
case 1:
#ifdef ERROR
std::cout<<PRINT<<"Read PacketSource POOrt:[ "<<packet->sourcePort
<<" ]:: Destination Port:[ "<<packet->destinationPort<<" ]"<<std::endl;
#endif
readCount++;
switch ( packet->sourcePort ){
case 0:
flag = !gInit();
break;
default:
if (! packetHandler ( packet ) ){
std::cerr<<PRINT<<"Error on packet grouping\n";
flag = true;
}
}
// readBufferObj->giveSignal();
// flag = true;
break;
case -1:
std::cerr<<PRINT<<"Error on read packet from buffer";
exit(1);
break;
case 0:
flag = readBufferObj->checkSignal();
std::this_thread::sleep_for ( std::chrono::milliseconds( 100 ) );
break;
default:
break;
//std::cout<<PRINT<<"Unhandled return Value from read Buffer \n";
}
}
std::cout<<PRINT<<"Total packet read from buffer: [ "<<readCount<< " ] \n";
}
int main()
{
// Initialize PortHandler Structs
// Set the port path
// Get methods and members of PortHandlerLinux or PortHandlerWindows
int port_num = portHandler(DEVICENAME);
// Initialize PacketHandler Structs
packetHandler();
// Initialize Groupsyncwrite instance
int groupwrite_num = groupSyncWrite(port_num, PROTOCOL_VERSION, ADDR_PRO_INDIRECTDATA_FOR_WRITE, LEN_PRO_INDIRECTDATA_FOR_WRITE);
// Initialize Groupsyncread instance
int groupread_num = groupSyncRead(port_num, PROTOCOL_VERSION, ADDR_PRO_INDIRECTDATA_FOR_READ, LEN_PRO_INDIRECTDATA_FOR_READ);
int index = 0;
int dxl_comm_result = COMM_TX_FAIL; // Communication result
uint8_t dxl_addparam_result = False; // AddParam result
uint8_t dxl_getdata_result = False; // GetParam result
int dxl_goal_position[2] = { DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE }; // Goal position
uint8_t dxl_error = 0; // Dynamixel error
uint8_t dxl_moving = 0; // Dynamixel moving status
uint8_t dxl_led_value[2] = { 0x00, 0xFF }; // Dynamixel LED value
int32_t dxl_present_position = 0; // Present position
// Open port
if (openPort(port_num))
{
printf("Succeeded to open the port!\n");
}
else
{
printf("Failed to open the port!\n");
printf("Press any key to terminate...\n");
getch();
return 0;
}
// Set port baudrate
if (setBaudRate(port_num, BAUDRATE))
{
printf("Succeeded to change the baudrate!\n");
}
else
{
printf("Failed to change the baudrate!\n");
printf("Press any key to terminate...\n");
getch();
return 0;
}
// Disable Dynamixel Torque :
// Indirect address would not accessible when the torque is already enabled
write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE);
if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
{
printTxRxResult(PROTOCOL_VERSION, dxl_comm_result);
}
else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
{
printRxPacketError(PROTOCOL_VERSION, dxl_error);
}
else
{
printf("DXL has been successfully connected \n");
}
// INDIRECTDATA parameter storages replace LED, goal position, present position and moving status storages
write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_WRITE + 0, ADDR_PRO_GOAL_POSITION + 0);
if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
{
printTxRxResult(PROTOCOL_VERSION, dxl_comm_result);
}
else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
{
printRxPacketError(PROTOCOL_VERSION, dxl_error);
}
write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_WRITE + 2, ADDR_PRO_GOAL_POSITION + 1);
if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
{
printTxRxResult(PROTOCOL_VERSION, dxl_comm_result);
}
else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
{
printRxPacketError(PROTOCOL_VERSION, dxl_error);
}
write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_WRITE + 4, ADDR_PRO_GOAL_POSITION + 2);
if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
{
printTxRxResult(PROTOCOL_VERSION, dxl_comm_result);
}
else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
{
printRxPacketError(PROTOCOL_VERSION, dxl_error);
}
write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_WRITE + 6, ADDR_PRO_GOAL_POSITION + 3);
if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
{
printTxRxResult(PROTOCOL_VERSION, dxl_comm_result);
}
else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
{
printRxPacketError(PROTOCOL_VERSION, dxl_error);
}
write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_WRITE + 8, ADDR_PRO_LED_RED);
if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
{
printTxRxResult(PROTOCOL_VERSION, dxl_comm_result);
}
else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
{
printRxPacketError(PROTOCOL_VERSION, dxl_error);
}
write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_READ + 0, ADDR_PRO_PRESENT_POSITION + 0);
if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
{
printTxRxResult(PROTOCOL_VERSION, dxl_comm_result);
}
else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
{
printRxPacketError(PROTOCOL_VERSION, dxl_error);
}
write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_READ + 2, ADDR_PRO_PRESENT_POSITION + 1);
if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
{
printTxRxResult(PROTOCOL_VERSION, dxl_comm_result);
}
else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
{
printRxPacketError(PROTOCOL_VERSION, dxl_error);
}
write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_READ + 4, ADDR_PRO_PRESENT_POSITION + 2);
if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
{
printTxRxResult(PROTOCOL_VERSION, dxl_comm_result);
}
else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
{
printRxPacketError(PROTOCOL_VERSION, dxl_error);
}
write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_READ + 6, ADDR_PRO_PRESENT_POSITION + 3);
if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
{
printTxRxResult(PROTOCOL_VERSION, dxl_comm_result);
}
else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
{
printRxPacketError(PROTOCOL_VERSION, dxl_error);
}
write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_READ + 8, ADDR_PRO_MOVING);
if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
{
printTxRxResult(PROTOCOL_VERSION, dxl_comm_result);
}
else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
{
printRxPacketError(PROTOCOL_VERSION, dxl_error);
}
// Enable Dynamixel Torque
write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE);
if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
{
printTxRxResult(PROTOCOL_VERSION, dxl_comm_result);
}
else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
{
printRxPacketError(PROTOCOL_VERSION, dxl_error);
}
// Add parameter storage for the present position value
dxl_addparam_result = groupSyncReadAddParam(groupread_num, DXL_ID);
if (dxl_addparam_result != True)
{
fprintf(stderr, "[ID:%03d] groupSyncRead addparam failed\n", DXL_ID);
return 0;
}
while (1)
{
printf("Press any key to continue! (or press ESC to quit!)\n");
if (getch() == ESC_ASCII_VALUE)
break;
// Add values to the Syncwrite storage
dxl_addparam_result = groupSyncWriteAddParam(groupwrite_num, DXL_ID, dxl_goal_position[index], 4);
if (dxl_addparam_result != True)
{
fprintf(stderr, "[ID:%03d] groupSyncWrite addparam failed\n", DXL_ID);
return 0;
}
dxl_addparam_result = groupSyncWriteAddParam(groupwrite_num, DXL_ID, dxl_led_value[index], 1);
if (dxl_addparam_result != True)
{
fprintf(stderr, "[ID:%03d] groupSyncWrite addparam failed\n", DXL_ID);
return 0;
}
// Syncwrite all
groupSyncWriteTxPacket(groupwrite_num);
if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
printTxRxResult(PROTOCOL_VERSION, dxl_comm_result);
// Clear syncwrite parameter storage
groupSyncWriteClearParam(groupwrite_num);
do
{
// Syncread present position from indirectdata2
groupSyncReadTxRxPacket(groupread_num);
if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
printTxRxResult(PROTOCOL_VERSION, dxl_comm_result);
// Check if groupsyncread data of Dyanamixel is available
dxl_getdata_result = groupSyncReadIsAvailable(groupread_num, DXL_ID, ADDR_PRO_INDIRECTDATA_FOR_READ, LEN_PRO_PRESENT_POSITION);
if (dxl_getdata_result != True)
{
fprintf(stderr, "[ID:%03d] groupSyncRead getdata failed", DXL_ID);
return 0;
}
// Check if groupsyncread data of Dyanamixel is available
dxl_getdata_result = groupSyncReadIsAvailable(groupread_num, DXL_ID, ADDR_PRO_INDIRECTDATA_FOR_READ + LEN_PRO_PRESENT_POSITION, LEN_PRO_MOVING);
if (dxl_getdata_result != True)
{
fprintf(stderr, "[ID:%03d] groupSyncRead getdata failed", DXL_ID);
return 0;
}
// Get Dynamixel present position value
dxl_present_position = groupSyncReadGetData(groupread_num, DXL_ID, ADDR_PRO_INDIRECTDATA_FOR_READ, LEN_PRO_PRESENT_POSITION);
// Get Dynamixel moving status value
dxl_moving = groupSyncReadGetData(groupread_num, DXL_ID, ADDR_PRO_INDIRECTDATA_FOR_READ + LEN_PRO_PRESENT_POSITION, LEN_PRO_MOVING);
printf("[ID:%03d] GoalPos:%d PresPos:%d IsMoving:%d\n", DXL_ID, dxl_goal_position[index], dxl_present_position, dxl_moving);
} while (abs(dxl_goal_position[index] - dxl_present_position) > DXL_MOVING_STATUS_THRESHOLD);
// Change goal position
if (index == 0)
{
index = 1;
}
else
{
index = 0;
}
}
// Disable Dynamixel Torque
write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE);
if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
{
printTxRxResult(PROTOCOL_VERSION, dxl_comm_result);
}
else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
{
printRxPacketError(PROTOCOL_VERSION, dxl_error);
}
// Close port
closePort(port_num);
return 0;
}
void Session::receive(PacketReader& packet) {
Logs::Dump(packet,format("Request from %s",peer.address.toString()).c_str(),nextDumpAreMiddle);
packetHandler(packet);
}
AREXPORT void ArPTZ::sensorInterpHandler(void)
{
ArBasePacket *packet;
while ((packet = readPacket()) != NULL)
packetHandler(packet);
}
