diff --git a/src/darwin_robot.cpp b/src/darwin_robot.cpp
index 9f512d987870e9aad6a8799213808b6be46839e0..ca6c6931b900f4e185e14d4fc176fa9ecf327f07 100644
--- a/src/darwin_robot.cpp
+++ b/src/darwin_robot.cpp
@@ -841,7 +841,7 @@ std::vector<double> CDarwinRobot::mm_get_servo_angles(void)
     this->robot_device->read_registers(DARWIN_MM_SERVO0_CUR_POS_L,(unsigned char *)values,MAX_NUM_SERVOS*2);
 
     for(i=0;i<MAX_NUM_SERVOS;i++)
-      angles[i]=((double)values[i])/128.0;
+      angles[i]=((double)(signed short int)values[i])/128.0;
 
     return angles;
   }
@@ -860,8 +860,8 @@ double CDarwinRobot::mm_get_servo_angle(unsigned char servo_id)
     {
       throw CDarwinRobotException(_HERE_,"Invalid servo identifier");
     }
-    this->robot_device->read_word_register(DARWIN_MM_SERVO0_CUR_POS_L+servo_id/2,&value);
-    angle=((double)value)/128.0;
+    this->robot_device->read_word_register(DARWIN_MM_SERVO0_CUR_POS_L+servo_id*2,&value);
+    angle=((double)(signed short int)value)/128.0;
 
     return angle;
   }
diff --git a/src/examples/darwin_imu_test.cpp b/src/examples/darwin_imu_test.cpp
index c48e3b4b2bd011d5f5ec63a86ed90726ed151e27..9c54c93f618fe0cf97fb8f938d13f54b9e86f1eb 100644
--- a/src/examples/darwin_imu_test.cpp
+++ b/src/examples/darwin_imu_test.cpp
@@ -3,8 +3,8 @@
 
 #include <iostream>
 
-//std::string robot_device="A603LOBS";
-std::string robot_device="A4008atn";
+std::string robot_device="/dev/pts/20";
+//std::string robot_device="A4008atn";
 
 int main(int argc, char *argv[])
 {
@@ -12,7 +12,7 @@ int main(int argc, char *argv[])
   double accel_x,accel_y,accel_z;
 
   try{
-    CDarwinRobot darwin("Darwin",robot_device,1000000,0x02);
+    CDarwinRobot darwin("Darwin",robot_device,1000000,0x02,true);
     std::cout << "found darwin controller" << std::endl;
     std::cout << "Number of calibration samples: " << darwin.imu_get_cal_samples() << std::endl;
     darwin.imu_start();
diff --git a/src/examples/darwin_joint_motion_test.cpp b/src/examples/darwin_joint_motion_test.cpp
index 6008e027282ec6d78b5400535eddd4c7cafe2214..e86d584d900a5e4331fa0aeb5d6955f1210e2cd2 100644
--- a/src/examples/darwin_joint_motion_test.cpp
+++ b/src/examples/darwin_joint_motion_test.cpp
@@ -45,6 +45,7 @@ int main(int argc, char *argv[])
       while(darwin.joints_are_moving(JOINTS_GRP0))
       {
         std::cout << "moving joints ..." << std::endl;
+        std::cout << "current angle: " << darwin.mm_get_servo_angle(19) << std::endl;
         usleep(100000);
       }
       servos.clear();
@@ -60,6 +61,7 @@ int main(int argc, char *argv[])
       while(darwin.joints_are_moving(JOINTS_GRP0))
       {
         std::cout << "moving joints ..." << std::endl;
+        std::cout << "current angle: " << darwin.mm_get_servo_angle(19) << std::endl;
         usleep(100000);
       }
     }