diff --git a/include/darwin_registers.h b/include/darwin_registers.h
index ea257d4e52ee34d18513120a7d80b84c764f60bf..689d39addadbfd53416ddbd829cf6d92d5573b55 100644
--- a/include/darwin_registers.h
+++ b/include/darwin_registers.h
@@ -1,105 +1,137 @@
 #ifndef _DARWIN_REGISTERS_H
 #define _DARWIN_REGISTERS_H
 
-#define MANAGER_PERIOD                6
-#define MANAGER_NUM_MODULES           128
-#define MANAGER_NUM_MASTERS           129
-#define MANAGER_CONTROL               130
-  #define MANAGER_START               0x01
-  #define MANAGER_STOP                0x02
-  #define MANAGER_START_SCAN          0x04
-  #define MANAGER_RUNNING             0x40
-  #define MANAGER_SCANNING            0x80
-#define MANAGER_NUM_DEVICES           131
-#define MANAGER_PRESENT_DEVICES       132
-
-#define MMANAGER_PERIOD               17
-#define MMANAGER_OFFSETS              18
-#define MMANAGER_NUM_MODELS           136
-#define MMANAGER_NUM_DEVICES          137
-#define MMANAGER_PRESENT_SERVOS       138
-#define MMANAGER_ENABLE               142
-  #define MMANAGER_EVEN_SER_EN        0x80
-  #define MMANAGER_EVEN_SER_MOD       0x70
-  #define MMANAGER_ODD_SER_EN         0x08
-  #define MMANAGER_ODD_SER_MOD        0x07
-#define MMANAGER_CURRENT_ANGLE        158
-
-#define ACTION_CONTROL                224
-  #define ACTION_START                0x01
-  #define ACTION_STOP                 0x02
-  #define ACTION_STATUS               0x80
-#define ACTION_PAGE_ID                225
-
-#define BALANCE_CONTROL_OFFSET        226//   bit 7  |   bit 6  |   bit 5  | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
-                                         // standing | bwd fall | fwd fall |       |       |       |       | enable
-  #define BALANCE_ENABLE              0x01
-  #define BALANCE_STANDING            0x80
-  #define BALANCE_BWD_FALL            0x40
-  #define BALANCE_FWD_FALL            0x20
-
-#define JOINT_MOTION_GROUP_CONTROL    227//   bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
-                                         // running |       |       |       |       |       | stop  | start
-  #define JOINT_MOTION_START          0x01
-  #define JOINT_MOTION_STOP           0x02
-  #define JOINT_MOTION_RUNNING        0x80
-
-#define JOINT_MOTION_GROUP_SERVOS     231 
-#define JOINT_MOTION_GROUP_ANGLES     247
-#define JOINT_MOTION_GROUP_SPEEDS     311
-#define JOINT_MOTION_GROUP_ACCELS     343
-
-#define HEAD_TRACKING_CONTROL         375// bit 7 | bit 6 | bit 5 |  bit 4   | bit 3 | bit 2 |     bit 1     |      bit 0
-                                         //       |       |       | tracking |       |       | stop tracking | start tracking
-  #define HEAD_TRACKING_START         0x01
-  #define HEAD_TRACKING_STOP          0x02
-  #define HEAD_TRACKING_RUNNING       0x10
-
-#define HEAD_TRACKING_MAX_PAN         376
-#define HEAD_TRACKING_MIN_PAN         378
-#define HEAD_TRACKING_TARGET_PAN      380
-#define HEAD_TRACKING_MAX_TILT        382
-#define HEAD_TRACKING_MIN_TILT        384
-#define HEAD_TRACKING_TARGET_TILT     386
-
-#define HEAD_TRACKING_PAN_P_GAIN      50
-#define HEAD_TRACKING_PAN_I_GAIN      52
-#define HEAD_TRACKING_PAN_D_GAIN      54
-#define HEAD_TRACKING_PAN_I_CLAMP     56
-#define HEAD_TRACKING_TILT_P_GAIN     58
-#define HEAD_TRACKING_TILT_I_GAIN     60
-#define HEAD_TRACKING_TILT_D_GAIN     62
-#define HEAD_TRACKING_TILT_I_CLAMP    64
-#define HEAD_TRACKING_PAN_SERVO_ID    66
-#define HEAD_TRACKING_TILT_SERVO_ID   68
-
-#define WALK_CONTROL                  388// bit 7 | bit 6 | bit 5 |  bit 4  | bit 3 | bit 2 |     bit 1    |    bit 0
-                                         // current phase           walking                   stop walking   start walking
-  #define WALK_START                  0x01
-  #define WALK_STOP                   0x02
-  #define WALK_RUNNING                0x10
-  #define WALK_CURRENT_PHASE          0xC0
-
-#define WALK_STEP_FW_BW               389
-#define WALK_STEP_LEFT_RIGHT          390
-#define WALK_STEP_DIRECTION           301
-
-#define IMU_CONTROL_OFFSET            392//   bit 7   |   bit 6  |    bit 5    |  bit 4  | bit 3 |    bit 2   | bit 1 | bit 0
-                                         // accel_det | gyro_det | calibrating | running |       | start_cal  | stop  | start
-  #define IMU_START                   0x01
-  #define IMU_STOP                    0x02
-  #define IMU_START_CAL               0x04 
-  #define IMU_RUNNING                 0x10
-  #define IMU_CALIBRATING             0x20
-  #define IMU_ACCEL_DET               0x40
-  #define IMU_GYRO_DET                0x80
-#define IMU_CAL_SAMPLES_OFFSET        393 
-#define IMU_GYRO_X_OFFSET             395
-#define IMU_GYRO_Y_OFFSET             397
-#define IMU_GYRO_Z_OFFSET             399
-#define IMU_ACCEL_X_OFFSET            401
-#define IMU_ACCEL_Y_OFFSET            403
-#define IMU_ACCEL_Z_OFFSET            405
+#define MANAGER_PERIOD                 6
+#define MANAGER_NUM_MODULES            128
+#define MANAGER_NUM_MASTERS            129
+#define MANAGER_CONTROL                130
+  #define MANAGER_START                0x01
+  #define MANAGER_STOP                 0x02
+  #define MANAGER_START_SCAN           0x04
+  #define MANAGER_RUNNING              0x40
+  #define MANAGER_SCANNING             0x80
+#define MANAGER_NUM_DEVICES            131
+#define MANAGER_PRESENT_DEVICES        132
+
+#define MMANAGER_PERIOD                17
+#define MMANAGER_OFFSETS               18
+#define MMANAGER_NUM_MODELS            136
+#define MMANAGER_NUM_DEVICES           137
+#define MMANAGER_PRESENT_SERVOS        138
+#define MMANAGER_ENABLE                142
+  #define MMANAGER_EVEN_SER_EN         0x80
+  #define MMANAGER_EVEN_SER_MOD        0x70
+  #define MMANAGER_ODD_SER_EN          0x08
+  #define MMANAGER_ODD_SER_MOD         0x07
+#define MMANAGER_CURRENT_ANGLE         158
+
+#define ACTION_CONTROL                 224
+  #define ACTION_START                 0x01
+  #define ACTION_STOP                  0x02
+  #define ACTION_STATUS                0x80
+#define ACTION_PAGE_ID                 225
+
+#define BALANCE_CONTROL_OFFSET         226//   bit 7  |   bit 6  |   bit 5  | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+                                          // standing | bwd fall | fwd fall |       |       |       |       | enable
+  #define BALANCE_ENABLE               0x01
+  #define BALANCE_STANDING             0x80
+  #define BALANCE_BWD_FALL             0x40
+  #define BALANCE_FWD_FALL             0x20
+
+#define JOINT_MOTION_GROUP_CONTROL     227//   bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+                                          // running |       |       |       |       |       | stop  | start
+  #define JOINT_MOTION_START           0x01
+  #define JOINT_MOTION_STOP            0x02
+  #define JOINT_MOTION_RUNNING         0x80
+
+#define JOINT_MOTION_GROUP_SERVOS      231 
+#define JOINT_MOTION_GROUP_ANGLES      247
+#define JOINT_MOTION_GROUP_SPEEDS      311
+#define JOINT_MOTION_GROUP_ACCELS      343
+
+#define HEAD_TRACKING_CONTROL          375// bit 7 | bit 6 | bit 5 |  bit 4   | bit 3 | bit 2 |     bit 1     |      bit 0
+                                          //       |       |       | tracking |       |       | stop tracking | start tracking
+  #define HEAD_TRACKING_START          0x01
+  #define HEAD_TRACKING_STOP           0x02
+  #define HEAD_TRACKING_RUNNING        0x10
+
+#define HEAD_TRACKING_MAX_PAN          376
+#define HEAD_TRACKING_MIN_PAN          378
+#define HEAD_TRACKING_TARGET_PAN       380
+#define HEAD_TRACKING_MAX_TILT         382
+#define HEAD_TRACKING_MIN_TILT         384
+#define HEAD_TRACKING_TARGET_TILT      386
+
+#define HEAD_TRACKING_PAN_P_GAIN       50
+#define HEAD_TRACKING_PAN_I_GAIN       52
+#define HEAD_TRACKING_PAN_D_GAIN       54
+#define HEAD_TRACKING_PAN_I_CLAMP      56
+#define HEAD_TRACKING_TILT_P_GAIN      58
+#define HEAD_TRACKING_TILT_I_GAIN      60
+#define HEAD_TRACKING_TILT_D_GAIN      62
+#define HEAD_TRACKING_TILT_I_CLAMP     64
+#define HEAD_TRACKING_PAN_SERVO_ID     66
+#define HEAD_TRACKING_TILT_SERVO_ID    67
+
+#define WALK_CONTROL                   388// bit 7 | bit 6 | bit 5 |  bit 4  | bit 3 | bit 2 |     bit 1    |    bit 0
+                                          // current phase           walking                   stop walking   start walking
+  #define WALK_START                   0x01
+  #define WALK_STOP                    0x02
+  #define WALK_RUNNING                 0x10
+  #define WALK_CURRENT_PHASE           0xC0
+
+#define WALK_STEP_FW_BW                389
+#define WALK_STEP_LEFT_RIGHT           390
+#define WALK_STEP_DIRECTION            301
+
+#define WALK_X_OFFSET                  68
+#define WALK_Y_OFFSET                  69
+#define WALK_Z_OFFSET                  70
+#define WALK_ROLL_OFFSET               71
+#define WALK_PITCH_OFFSET              72
+#define WALK_YAW_OFFSET                73
+#define WALK_HIP_PITCH_OFFSET          74
+#define WALK_PERIOD_TIME               76
+#define WALK_DSP_RATIO                 78
+#define WALK_STEP_FW_BW_RATIO          79
+#define WALK_FOOT_HEIGHT               80
+#define WALK_SWING_RIGHT_LEFT          81
+#define WALK_SWING_TOP_DOWN            82
+#define WALK_PELVIS_OFFSET             83
+#define WALK_ARM_SWING_GAIN            84
+#define WALK_MAX_VEL                   85
+#define WALK_MAX_ROT_VEL               86
+#define WALK_R_HIP_YAW_SERVO_ID        87
+#define WALK_R_HIP_ROLL_SERVO_ID       88
+#define WALK_R_HIP_PITCH_SERVO_ID      89
+#define WALK_R_KNEE_SERVO_ID           90
+#define WALK_R_ANKLE_PITCH_SERVO_ID    91
+#define WALK_R_ANKLE_ROLL_SERVO_ID     92
+#define WALK_R_SHOULDER_PITCH_SERVO_ID 93
+#define WALK_L_HIP_YAW_SERVO_ID        94
+#define WALK_L_HIP_ROLL_SERVO_ID       95
+#define WALK_L_HIP_PITCH_SERVO_ID      96
+#define WALK_L_KNEE_SERVO_ID           97
+#define WALK_L_ANKLE_PITCH_SERVO_ID    98
+#define WALK_L_ANKLE_ROLL_SERVO_ID     99
+#define WALK_L_SHOULDER_PITCH_SERVO_ID 100
+
+#define IMU_CONTROL_OFFSET             392//   bit 7   |   bit 6  |    bit 5    |  bit 4  | bit 3 |    bit 2   | bit 1 | bit 0
+                                          // accel_det | gyro_det | calibrating | running |       | start_cal  | stop  | start
+  #define IMU_START                    0x01
+  #define IMU_STOP                     0x02
+  #define IMU_START_CAL                0x04 
+  #define IMU_RUNNING                  0x10
+  #define IMU_CALIBRATING              0x20
+  #define IMU_ACCEL_DET                0x40
+  #define IMU_GYRO_DET                 0x80
+#define IMU_CAL_SAMPLES_OFFSET         393 
+#define IMU_GYRO_X_OFFSET              395
+#define IMU_GYRO_Y_OFFSET              397
+#define IMU_GYRO_Z_OFFSET              399
+#define IMU_ACCEL_X_OFFSET             401
+#define IMU_ACCEL_Y_OFFSET             403
+#define IMU_ACCEL_Z_OFFSET             405
 
 #define BALANCE_KNEE_GAIN_OFFSET        7 
 #define BALANCE_ANKLE_ROLL_GAIN_OFFSET  9