diff --git a/src/dynamixelserver.cpp b/src/dynamixelserver.cpp
index d9c6cd2b599996aa4d4e17a2cefce08a1513fc1c..dc250af1d653d8a89ac4ec00ff9028e0711bee40 100644
--- a/src/dynamixelserver.cpp
+++ b/src/dynamixelserver.cpp
@@ -2,6 +2,7 @@
#include "dynamixelserver.h"
#include "eventexceptions.h"
#include <sstream>
+#include <iostream>
const unsigned short crc_table[256] = {
0x0000, 0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011,
@@ -596,29 +597,28 @@ void *CDynamixelServer::scan_thread(void *param)
dyn_server->state=dyn_created;
dyn_server->scan_error="communication device not initialized";
dyn_server->event_server->set_event(dyn_server->scan_error_event_id);
- dyn_server->close();
dyn_server->dynamixel_access.exit();
pthread_exit(NULL);
}
for(freq=0;freq<9;freq++)
{
+ std::cout << "trying frequency: " << frequencies[freq] << std::endl;
dyn_server->set_baudrate(frequencies[freq]);
for(id=0;id<0xFD;id++)
{
if(dyn_server->event_server->event_is_set(dyn_server->stop_scan_event_id))
{
dyn_server->event_server->reset_event(dyn_server->stop_scan_event_id);
- if(dyn_server->scan_version==dyn_version1)
- dyn_server->devices_v1.clear();
- else
- dyn_server->devices_v2.clear();
- dyn_server->close();
+ dyn_server->devices_v1.clear();
+ dyn_server->devices_v2.clear();
pthread_exit(NULL);
}
else
{
try{
+ std::cout << "checking device ID: " << id << std::endl;
dyn_server->ping(id,20,dyn_server->scan_version);
+ std::cout << "Found device with ID: " << id << std::endl;
}catch(CEventTimeoutException &e){
continue;
}
@@ -643,14 +643,11 @@ void *CDynamixelServer::scan_thread(void *param)
end=true;// nothing found
}catch(CException &e){
dyn_server->dynamixel_access.enter();
- if(dyn_server->scan_version==dyn_version1)
- dyn_server->devices_v1.clear();
- else
- dyn_server->devices_v2.clear();
+ dyn_server->devices_v1.clear();
+ dyn_server->devices_v2.clear();
dyn_server->state=dyn_created;
dyn_server->scan_error=e.what();
dyn_server->event_server->set_event(dyn_server->scan_error_event_id);
- dyn_server->close();
dyn_server->dynamixel_access.exit();
pthread_exit(NULL);
}
@@ -673,11 +670,8 @@ void CDynamixelServer::start_scan(dyn_version_t version)
{
this->scan_version=version;
this->stop_scan();
- if(version==dyn_version1)
- this->devices_v1.clear();
- else
- this->devices_v2.clear();
- this->close();
+ this->devices_v1.clear();
+ this->devices_v2.clear();
this->thread_server->start_thread(this->scan_thread_id);
}
}
@@ -899,95 +893,26 @@ void CDynamixelServer::action(dyn_version_t version)
void CDynamixelServer::ping(int dev_id,int time,dyn_version_t version,unsigned short *model,unsigned char *fw_ver)
{
- unsigned char *data,length_v1,dummy;
- std::list<std::string> events;
+ unsigned char *data,length_v1;
unsigned short length_v2;
- int num_retries=5,num,i;
- bool done=false;
if(version==dyn_version1)
{
- events.push_back(this->comm_dev->get_rx_event_id());
- while(!done)
- {
- try{
- this->send_instruction_packet_v1(dyn_ping,NULL,0,dev_id);
- this->receive_status_packet_v1(&data,&length_v1);
- if(data!=NULL)
- delete[] data;
- done=true;
- }catch(CEventTimeoutException &e){
- this->dynamixel_access.enter();
- for(i=0;i<32;i++)
- {
- if((num=this->comm_dev->get_num_data())==0)
- {
- try{
- this->comm_dev->write(&dummy,1);
- this->event_server->wait_all(events,20);
- }catch(CEventTimeoutException &e){
- }
- }
- else
- break;
- }
- this->dynamixel_access.exit();
- if(num>0)
- {
- this->receive_status_packet_v1(&data,&length_v1);
- if(data!=NULL)
- delete[] data;
- usleep(100000);
- }
- num_retries--;
- if(num_retries==0)
- throw e;
- }
- }
+ this->send_instruction_packet_v1(dyn_ping,NULL,0,dev_id);
+ this->receive_status_packet_v1(&data,&length_v1);
+ if(data!=NULL)
+ delete[] data;
}
else
{
- events.push_back(this->comm_dev->get_rx_event_id());
- while(!done)
- {
- try{
- this->send_instruction_packet_v2(dyn_ping,NULL,0,dev_id);
- this->receive_status_packet_v2(&data,&length_v2);
- if(model!=NULL)
- *model=data[0]+data[1]*256;
- if(fw_ver!=NULL)
- *fw_ver=data[2];
- if(data!=NULL)
- delete[] data;
- done=true;
- }catch(CEventTimeoutException &e){
- this->dynamixel_access.enter();
- for(i=0;i<32;i++)
- {
- if((num=this->comm_dev->get_num_data())==0)
- {
- try{
- this->comm_dev->write(&dummy,1);
- this->event_server->wait_all(events,20);
- }catch(CEventTimeoutException &e){
- }
- }
- else
- break;
- }
- this->dynamixel_access.exit();
- if(num>0)
- {
- this->receive_status_packet_v1(&data,&length_v1);
- if(data!=NULL)
- delete[] data;
- usleep(100000);
- }
- num_retries--;
- if(num_retries==0)
- throw e;
- }
- }
+ this->send_instruction_packet_v2(dyn_ping,NULL,0,dev_id);
+ this->receive_status_packet_v2(&data,&length_v2);
+ if(model!=NULL)
+ *model=data[0]+data[1]*256;
+ if(fw_ver!=NULL)
+ *fw_ver=data[2];
+ if(data!=NULL)
+ delete[] data;
}
}
diff --git a/src/examples/test_dynamixel_slave.cpp b/src/examples/test_dynamixel_slave.cpp
index 4859730690a13c541fa1e4dcd7ab8ca5f9ab48dd..1f269e340055acf536707fa8cfa4944464b1500a 100644
--- a/src/examples/test_dynamixel_slave.cpp
+++ b/src/examples/test_dynamixel_slave.cpp
@@ -5,8 +5,8 @@
int main(int argc, char *argv[])
{
try{
- CDynamixelSlaveSerial slave("slave","/dev/ttyUSB0");
- slave.set_baudrate(921600);
+ CDynamixelSlaveSerial slave("slave","/dev/ttyUSB1");
+ slave.set_baudrate(1000000);
sleep(10);
std::cout << "exit" << std::endl;
}catch(CException &e){