diff --git a/dynamixel_base/src/dynamixel_master.c b/dynamixel_base/src/dynamixel_master.c
index 7295fe5bdbe28b8e695e48233a4414aaf14c6643..0d38df5832a5ca9dc93e747e31ba1d8d710a2ca9 100644
--- a/dynamixel_base/src/dynamixel_master.c
+++ b/dynamixel_base/src/dynamixel_master.c
@@ -181,7 +181,17 @@ inline return_level_t dyn_master_get_return_level(TDynamixelMaster *master)
 
 void dyn_master_scan(TDynamixelMaster *master,unsigned char *num,unsigned char *ids)
 {
+  unsigned char i;
 
+  *num=0;
+  for(i=0;i<254;i++)
+  {
+    if(dyn_master_ping(master,i)==DYN_SUCCESS)// the device exists
+    {
+      ids[*num]=i;
+      (*num)++;
+    }
+  }
 }
 
 unsigned char dyn_master_ping(TDynamixelMaster *master,unsigned char id)