Added some exception handling that was missing.

Added a synchronization procedure in the dynamixel server to avoid problems at first power up.

src/dynamixel.cpp

@@ -184,6 +184,9 @@ unsigned char CDynamixel::receive_status_packet_v1(unsigned char **data,unsigned
     }catch(CEventTimeoutException &e){
       this->usb_access->exit();
       throw e;
+    }catch(CException &e){
+      this->usb_access->exit();
+      throw e;
     }
   }
   else
@@ -269,6 +272,9 @@ unsigned char CDynamixel::receive_status_packet_v2(unsigned char **data,unsigned
     }catch(CEventTimeoutException &e){
       this->usb_access->exit();
       throw e;
+    }catch(CException &e){
+      this->usb_access->exit();
+      throw e;
     }
   }
   else

src/dynamixelserver.cpp

@@ -116,18 +116,22 @@ void CDynamixelServer::send_instruction_packet_v1(dyn_inst_t inst,unsigned char
   packet[length-1]=this->compute_checksum_v1(packet,length);
   if(this->comm_dev!=NULL)
   {
-    this->dynamixel_access.enter();
-    if(this->comm_dev->write(packet,length)!=length)
-    {
-      /* handle exceptions */
+    try{
+      this->dynamixel_access.enter();
+      if(this->comm_dev->write(packet,length)!=length)
+      {
+        /* handle exceptions */
+        if(packet!=NULL)
+          delete[] packet;
+        throw CDynamixelServerException(_HERE_,"Unexpected error while writing to the communication device");
+      }
       if(packet!=NULL)
         delete[] packet;
       this->dynamixel_access.exit();
-      throw CDynamixelServerException(_HERE_,"Unexpected error while writing to the communication device");
+    }catch(CException &e){
+      this->dynamixel_access.exit();
+      throw e;
     }
-    if(packet!=NULL)
-      delete[] packet;
-    this->dynamixel_access.exit();
   }
   else
   {
@@ -177,19 +181,23 @@ void CDynamixelServer::send_instruction_packet_v2(dyn_inst_t inst,unsigned char
   packet[length-2]=crc%256;
   packet[length-1]=crc/256;
   if(this->comm_dev!=NULL)
-  {
-    this->dynamixel_access.enter();
-    if(this->comm_dev->write(packet,length)!=length)
-    {
-      /* handle exceptions */
+  { 
+    try{
+      this->dynamixel_access.enter();
+      if(this->comm_dev->write(packet,length)!=length)
+      {
+        /* handle exceptions */
+        if(packet!=NULL)
+          delete[] packet;
+        throw CDynamixelServerException(_HERE_,"Unexpected error while writing to the communication device");
+      }
       if(packet!=NULL)
         delete[] packet;
       this->dynamixel_access.exit();
-      throw CDynamixelServerException(_HERE_,"Unexpected error while writing to the communication device");
+    }catch(CException &e){
+      this->dynamixel_access.exit();
+      throw e; 
     }
-    if(packet!=NULL)
-      delete[] packet;
-    this->dynamixel_access.exit();
   }
   else
   {
@@ -253,7 +261,6 @@ unsigned char CDynamixelServer::receive_status_packet_v1(unsigned char **data,un
       // check the checksum
       if(this->compute_checksum_v1(&data_int[start],length)!=0x00)
       {
-        this->dynamixel_access.exit();
         /* handle exceptions */
         throw CDynamixelServerException(_HERE_,"Invalid Checksum");
       }
@@ -275,6 +282,9 @@ unsigned char CDynamixelServer::receive_status_packet_v1(unsigned char **data,un
     }catch(CEventTimeoutException &e){
       this->dynamixel_access.exit();
       throw e;
+    }catch(CException &e){
+      this->dynamixel_access.exit();
+      throw e;
     }
   }
   else
@@ -339,7 +349,6 @@ unsigned char CDynamixelServer::receive_status_packet_v2(unsigned char **data,un
       crc=this->compute_checksum_v2(&data_int[start],length-2);
       if((crc%256)!=data_int[start+length-2] || (crc/256)!=data_int[start+length-1])
       {
-        this->dynamixel_access.exit();
         /* handle exceptions */
         throw CDynamixelServerException(_HERE_,"Invalid Checksum");
       }
@@ -360,6 +369,9 @@ unsigned char CDynamixelServer::receive_status_packet_v2(unsigned char **data,un
     }catch(CEventTimeoutException &e){
       this->dynamixel_access.exit();
       throw e;
+    }catch(CException &e){
+      this->dynamixel_access.exit();
+      throw e;
     }
   }
   else
@@ -498,7 +510,6 @@ void CDynamixelServer::handle_error(unsigned char error)
       error_msg+="\nOverload";
     if(error&0x40)
       error_msg+="\nInvalid instruction";
-    std::cout << error_msg << std::endl;
     throw CDynamixelAlarmException(_HERE_,error_msg,0xFE,error);
   }
 }
@@ -1134,26 +1145,94 @@ 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;
+  unsigned char *data,length_v1,dummy;
+  std::list<std::string> events;
   unsigned short length_v2;
+  int num_retries=5,num,i;
+  bool done=false;
 
   if(version==dyn_version1)
   {
-    this->send_instruction_packet_v1(dyn_ping,NULL,0,dev_id);
-    this->receive_status_packet_v1(&data,&length_v1);
-    if(data!=NULL)
-      delete[] data;
+    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;
+      }
+    }
   }
   else
   {
-    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;
+    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;
+      }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;
+      }
+    }
   }
 }