diff --git a/Makefile b/Makefile
index b45c2be89f203cee089eeae8b20f08e8abae2ec6..62d4f0ce3d6e6b5d297e25a0dfbb363f1dc157c3 100755
--- a/Makefile
+++ b/Makefile
@@ -1,8 +1,8 @@
 # setup
 # modified by zerom for WinARM 8/2010
 
-STM32_HAL_PATH=/home/shernand/humanoids/stm32_hal
-STM32_LIBRARIES_PATH=/home/shernand/humanoids/stm32_libraries
+STM32_HAL_PATH=/home/sergi/humanoids/stm32_hal
+STM32_LIBRARIES_PATH=/home/sergi/humanoids/stm32_libraries
 
 PROJECT_NAME=darwin_firmware
 #TARGET_FILES=$(wildcard src/*.c)
@@ -23,6 +23,8 @@ TARGET_FILES+=src/motion_pages.c
 TARGET_FILES+=src/walking.c
 TARGET_FILES+=src/darwin_math.c
 TARGET_FILES+=src/darwin_kinematics.c
+TARGET_FILES+=src/joint_motion.c
+TARGET_FILES+=src/head_tracking.c
 
 TARGET_PROCESSOR=STM32F103RE
 
diff --git a/include/darwin_registers.h b/include/darwin_registers.h
index 9c19ae0680929e6c287d7ae6ab9c03829d2166a7..f9f515baeb8fb0232bd19df8f03050d9c5230cb2 100644
--- a/include/darwin_registers.h
+++ b/include/darwin_registers.h
@@ -65,6 +65,14 @@ extern "C" {
 #define WALK_ARM_SWING_GAIN             ((unsigned short int)0x0041)
 #define WALK_MAX_VEL                    ((unsigned short int)0x0042)
 #define WALK_MAX_ROT_VEL                ((unsigned short int)0x0043)
+#define HEAD_PAN_P                      ((unsigned short int)0x0044)
+#define HEAD_PAN_I                      ((unsigned short int)0x0046)
+#define HEAD_PAN_D                      ((unsigned short int)0x0048)
+#define HEAD_PAN_I_CLAMP                ((unsigned short int)0x004A)
+#define HEAD_TILT_P                     ((unsigned short int)0x004C)
+#define HEAD_TILT_I                     ((unsigned short int)0x004E)
+#define HEAD_TILT_D                     ((unsigned short int)0x0050)
+#define HEAD_TILT_I_CLAMP               ((unsigned short int)0x0052)
 
 #define LAST_EEPROM_OFFSET              ((unsigned short int)0x00FF)
 
@@ -137,6 +145,22 @@ typedef enum {
   DARWIN_WALK_ARM_SWING_GAIN       = WALK_ARM_SWING_GAIN,
   DARWIN_WALK_MAX_VEL              = WALK_MAX_VEL,
   DARWIN_WALK_MAX_ROT_VEL          = WALK_MAX_ROT_VEL,
+  DARWIN_HEAD_PAN_P_L              = HEAD_PAN_P,// constant in fixed point format 0|16 
+  DARWIN_HEAD_PAN_P_H              = HEAD_PAN_P+1,
+  DARWIN_HEAD_PAN_I_L              = HEAD_PAN_I,// constant in fixed point format 0|16 
+  DARWIN_HEAD_PAN_I_H              = HEAD_PAN_I+1,
+  DARWIN_HEAD_PAN_D_L              = HEAD_PAN_D,// constant in fixed point format 0|16 
+  DARWIN_HEAD_PAN_D_H              = HEAD_PAN_D+1,
+  DARWIN_HEAD_PAN_I_CLAMP_L        = HEAD_PAN_I_CLAMP,// max error in fixed point format 9|7 
+  DARWIN_HEAD_PAN_I_CLAMP_H        = HEAD_PAN_I_CLAMP+1,
+  DARWIN_HEAD_TILT_P_L             = HEAD_TILT_P,// constant in fixed point format 0|16 
+  DARWIN_HEAD_TILT_P_H             = HEAD_TILT_P+1,
+  DARWIN_HEAD_TILT_I_L             = HEAD_TILT_I,// constant in fixed point format 0|16 
+  DARWIN_HEAD_TILT_I_H             = HEAD_TILT_I+1,
+  DARWIN_HEAD_TILT_D_L             = HEAD_TILT_D,// constant in fixed point format 0|16 
+  DARWIN_HEAD_TILT_D_H             = HEAD_TILT_D+1,
+  DARWIN_HEAD_TILT_I_CLAMP_L       = HEAD_TILT_I_CLAMP,// max error in fixed point format 9|7 
+  DARWIN_HEAD_TILT_I_CLAMP_H       = HEAD_TILT_I_CLAMP+1,
   DARWIN_TX_LED_CNTRL              = 0x0100, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2  | bit 1 |      bit 0
                                              //       |       |       |       | blink | toggle | value | internally used
   DARWIN_TX_LED_PERIOD_L           = 0x0101,
@@ -337,7 +361,189 @@ typedef enum {
                                              // current phase           walking                   stop walking   start walking
   DARWIN_WALK_STEP_FW_BW           = 0x019F,
   DARWIN_WALK_STEP_LEFT_RIGHT      = 0x01A0,
-  DARWIN_WALK_STEP_DIRECTION       = 0x01A1
+  DARWIN_WALK_STEP_DIRECTION       = 0x01A1,
+  DARWIN_JOINT_GRP0_CNTRL          = 0x01A2,// bit 7 | bit 6 | bit 5 |      bit 4    | bit 3 | bit 2 |    bit 1   |   bit 0
+                                            //       |       |       | joints moving |       |       | stop joint | start joint
+  DARWIN_JOINT_GRP0_SERVOS0        = 0x01A3,// bit 7  | bit 6  | bit 5  | bit 4  | bit 3  | bit 2  | bit 1  | bit 0
+                                            // servo7 | servo6 | servo5 | servo4 | servo3 | servo2 | servo1 | servo0
+  DARWIN_JOINT_GRP0_SERVOS1        = 0x01A4,//  bit 7  |  bit 6  |  bit 5  |  bit 4  |  bit 3  |  bit 2  | bit 1  | bit 0
+                                            // servo15 | servo14 | servo13 | servo12 | servo11 | servo10 | servo9 | servo8
+  DARWIN_JOINT_GRP0_SERVOS2        = 0x01A5,//  bit 7  |  bit 6  |  bit 5  |  bit 4  |  bit 3  |  bit 2  |  bit 1  |  bit 0
+                                            // servo23 | servo22 | servo21 | servo20 | servo19 | servo18 | servo17 | servo16
+  DARWIN_JOINT_GRP0_SERVOS3        = 0x01A6,//  bit 7  |  bit 6  |  bit 5  |  bit 4  |  bit 3  |  bit 2  |  bit 1  |  bit 0
+                                            // servo31 | servo30 | servo29 | servo28 | servo27 | servo26 | servo25 | servo24
+  DARWIN_JOINT_GRP1_CNTRL          = 0x01A7,// bit 7 | bit 6 | bit 5 |      bit 4    | bit 3 | bit 2 |    bit 1   |   bit 0
+                                            //       |       |       | joints moving |       |       | stop joint | start joint
+  DARWIN_JOINT_GRP1_SERVOS0        = 0x01A8,// bit 7  | bit 6  | bit 5  | bit 4  | bit 3  | bit 2  | bit 1  | bit 0
+                                            // servo7 | servo6 | servo5 | servo4 | servo3 | servo2 | servo1 | servo0
+  DARWIN_JOINT_GRP1_SERVOS1        = 0x01A9,//  bit 7  |  bit 6  |  bit 5  |  bit 4  |  bit 3  |  bit 2  | bit 1  | bit 0
+                                            // servo15 | servo14 | servo13 | servo12 | servo11 | servo10 | servo9 | servo8
+  DARWIN_JOINT_GRP1_SERVOS2        = 0x01AA,//  bit 7  |  bit 6  |  bit 5  |  bit 4  |  bit 3  |  bit 2  |  bit 1  |  bit 0
+                                            // servo23 | servo22 | servo21 | servo20 | servo19 | servo18 | servo17 | servo16
+  DARWIN_JOINT_GRP1_SERVOS3        = 0x01AB,//  bit 7  |  bit 6  |  bit 5  |  bit 4  |  bit 3  |  bit 2  |  bit 1  |  bit 0
+                                            // servo31 | servo30 | servo29 | servo28 | servo27 | servo26 | servo25 | servo24
+  DARWIN_JOINT_GRP2_CNTRL          = 0x01AC,// bit 7 | bit 6 | bit 5 |      bit 4    | bit 3 | bit 2 |    bit 1   |   bit 0
+                                            //       |       |       | joints moving |       |       | stop joint | start joint
+  DARWIN_JOINT_GRP2_SERVOS0        = 0x01AD,// bit 7  | bit 6  | bit 5  | bit 4  | bit 3  | bit 2  | bit 1  | bit 0
+                                            // servo7 | servo6 | servo5 | servo4 | servo3 | servo2 | servo1 | servo0
+  DARWIN_JOINT_GRP2_SERVOS1        = 0x01AE,//  bit 7  |  bit 6  |  bit 5  |  bit 4  |  bit 3  |  bit 2  | bit 1  | bit 0
+                                            // servo15 | servo14 | servo13 | servo12 | servo11 | servo10 | servo9 | servo8
+  DARWIN_JOINT_GRP2_SERVOS2        = 0x01AF,//  bit 7  |  bit 6  |  bit 5  |  bit 4  |  bit 3  |  bit 2  |  bit 1  |  bit 0
+                                            // servo23 | servo22 | servo21 | servo20 | servo19 | servo18 | servo17 | servo16
+  DARWIN_JOINT_GRP2_SERVOS3        = 0x01B0,//  bit 7  |  bit 6  |  bit 5  |  bit 4  |  bit 3  |  bit 2  |  bit 1  |  bit 0
+                                            // servo31 | servo30 | servo29 | servo28 | servo27 | servo26 | servo25 | servo24
+  DARWIN_JOINT_GRP3_CNTRL          = 0x01B1,// bit 7 | bit 6 | bit 5 |      bit 4    | bit 3 | bit 2 |    bit 1   |   bit 0
+                                            //       |       |       | joints moving |       |       | stop joint | start joint
+  DARWIN_JOINT_GRP3_SERVOS0        = 0x01B2,// bit 7  | bit 6  | bit 5  | bit 4  | bit 3  | bit 2  | bit 1  | bit 0
+                                            // servo7 | servo6 | servo5 | servo4 | servo3 | servo2 | servo1 | servo0
+  DARWIN_JOINT_GRP3_SERVOS1        = 0x01B3,//  bit 7  |  bit 6  |  bit 5  |  bit 4  |  bit 3  |  bit 2  | bit 1  | bit 0
+                                            // servo15 | servo14 | servo13 | servo12 | servo11 | servo10 | servo9 | servo8
+  DARWIN_JOINT_GRP3_SERVOS2        = 0x01B4,//  bit 7  |  bit 6  |  bit 5  |  bit 4  |  bit 3  |  bit 2  |  bit 1  |  bit 0
+                                            // servo23 | servo22 | servo21 | servo20 | servo19 | servo18 | servo17 | servo16
+  DARWIN_JOINT_GRP3_SERVOS3        = 0x01B5,//  bit 7  |  bit 6  |  bit 5  |  bit 4  |  bit 3  |  bit 2  |  bit 1  |  bit 0
+                                            // servo31 | servo30 | servo29 | servo28 | servo27 | servo26 | servo25 | servo24
+  DARWIN_JOINT_SERVO0_ANGLE_L      = 0x01B6, // angle in fixed point format 9|7
+  DARWIN_JOINT_SERVO0_ANGLE_H      = 0x01B7, 
+  DARWIN_JOINT_SERVO1_ANGLE_L      = 0x01B8, // angle in fixed point format 9|7
+  DARWIN_JOINT_SERVO1_ANGLE_H      = 0x01B9, 
+  DARWIN_JOINT_SERVO2_ANGLE_L      = 0x01BA, // angle in fixed point format 9|7
+  DARWIN_JOINT_SERVO2_ANGLE_H      = 0x01BB, 
+  DARWIN_JOINT_SERVO3_ANGLE_L      = 0x01BC, // angle in fixed point format 9|7
+  DARWIN_JOINT_SERVO3_ANGLE_H      = 0x01BD, 
+  DARWIN_JOINT_SERVO4_ANGLE_L      = 0x01BE, // angle in fixed point format 9|7
+  DARWIN_JOINT_SERVO4_ANGLE_H      = 0x01BF, 
+  DARWIN_JOINT_SERVO5_ANGLE_L      = 0x01C0, // angle in fixed point format 9|7
+  DARWIN_JOINT_SERVO5_ANGLE_H      = 0x01C1, 
+  DARWIN_JOINT_SERVO6_ANGLE_L      = 0x01C2, // angle in fixed point format 9|7
+  DARWIN_JOINT_SERVO6_ANGLE_H      = 0x01C3, 
+  DARWIN_JOINT_SERVO7_ANGLE_L      = 0x01C4, // angle in fixed point format 9|7
+  DARWIN_JOINT_SERVO7_ANGLE_H      = 0x01C5, 
+  DARWIN_JOINT_SERVO8_ANGLE_L      = 0x01C6, // angle in fixed point format 9|7
+  DARWIN_JOINT_SERVO8_ANGLE_H      = 0x01C7, 
+  DARWIN_JOINT_SERVO9_ANGLE_L      = 0x01C8, // angle in fixed point format 9|7
+  DARWIN_JOINT_SERVO9_ANGLE_H      = 0x01C9, 
+  DARWIN_JOINT_SERVO10_ANGLE_L     = 0x01CA, // angle in fixed point format 9|7
+  DARWIN_JOINT_SERVO10_ANGLE_H     = 0x01CB, 
+  DARWIN_JOINT_SERVO11_ANGLE_L     = 0x01CC, // angle in fixed point format 9|7
+  DARWIN_JOINT_SERVO11_ANGLE_H     = 0x01CD, 
+  DARWIN_JOINT_SERVO12_ANGLE_L     = 0x01CE, // angle in fixed point format 9|7
+  DARWIN_JOINT_SERVO12_ANGLE_H     = 0x01CF, 
+  DARWIN_JOINT_SERVO13_ANGLE_L     = 0x01D0, // angle in fixed point format 9|7
+  DARWIN_JOINT_SERVO13_ANGLE_H     = 0x01D1, 
+  DARWIN_JOINT_SERVO14_ANGLE_L     = 0x01D2, // angle in fixed point format 9|7
+  DARWIN_JOINT_SERVO14_ANGLE_H     = 0x01D3, 
+  DARWIN_JOINT_SERVO15_ANGLE_L     = 0x01D4, // angle in fixed point format 9|7
+  DARWIN_JOINT_SERVO15_ANGLE_H     = 0x01D5, 
+  DARWIN_JOINT_SERVO16_ANGLE_L     = 0x01D6, // angle in fixed point format 9|7
+  DARWIN_JOINT_SERVO16_ANGLE_H     = 0x01D7, 
+  DARWIN_JOINT_SERVO17_ANGLE_L     = 0x01D8, // angle in fixed point format 9|7
+  DARWIN_JOINT_SERVO17_ANGLE_H     = 0x01D9, 
+  DARWIN_JOINT_SERVO18_ANGLE_L     = 0x01DA, // angle in fixed point format 9|7
+  DARWIN_JOINT_SERVO18_ANGLE_H     = 0x01DB, 
+  DARWIN_JOINT_SERVO19_ANGLE_L     = 0x01DC, // angle in fixed point format 9|7
+  DARWIN_JOINT_SERVO19_ANGLE_H     = 0x01DD, 
+  DARWIN_JOINT_SERVO20_ANGLE_L     = 0x01DE, // angle in fixed point format 9|7
+  DARWIN_JOINT_SERVO20_ANGLE_H     = 0x01DF, 
+  DARWIN_JOINT_SERVO21_ANGLE_L     = 0x01E0, // angle in fixed point format 9|7
+  DARWIN_JOINT_SERVO21_ANGLE_H     = 0x01E1, 
+  DARWIN_JOINT_SERVO22_ANGLE_L     = 0x01E2, // angle in fixed point format 9|7
+  DARWIN_JOINT_SERVO22_ANGLE_H     = 0x01E3, 
+  DARWIN_JOINT_SERVO23_ANGLE_L     = 0x01E4, // angle in fixed point format 9|7
+  DARWIN_JOINT_SERVO23_ANGLE_H     = 0x01E5, 
+  DARWIN_JOINT_SERVO24_ANGLE_L     = 0x01E6, // angle in fixed point format 9|7
+  DARWIN_JOINT_SERVO24_ANGLE_H     = 0x01E7, 
+  DARWIN_JOINT_SERVO25_ANGLE_L     = 0x01E8, // angle in fixed point format 9|7
+  DARWIN_JOINT_SERVO25_ANGLE_H     = 0x01E9, 
+  DARWIN_JOINT_SERVO26_ANGLE_L     = 0x01EA, // angle in fixed point format 9|7
+  DARWIN_JOINT_SERVO26_ANGLE_H     = 0x01EB, 
+  DARWIN_JOINT_SERVO27_ANGLE_L     = 0x01EC, // angle in fixed point format 9|7
+  DARWIN_JOINT_SERVO27_ANGLE_H     = 0x01ED, 
+  DARWIN_JOINT_SERVO28_ANGLE_L     = 0x01EE, // angle in fixed point format 9|7
+  DARWIN_JOINT_SERVO28_ANGLE_H     = 0x01EF, 
+  DARWIN_JOINT_SERVO29_ANGLE_L     = 0x01F0, // angle in fixed point format 9|7
+  DARWIN_JOINT_SERVO29_ANGLE_H     = 0x01F1, 
+  DARWIN_JOINT_SERVO30_ANGLE_L     = 0x01F2, // angle in fixed point format 9|7
+  DARWIN_JOINT_SERVO30_ANGLE_H     = 0x01F3, 
+  DARWIN_JOINT_SERVO31_ANGLE_L     = 0x01F4, // angle in fixed point format 9|7
+  DARWIN_JOINT_SERVO31_ANGLE_H     = 0x01F5, 
+  DARWIN_JOINT_SERVO0_SPEED        = 0x01F6, // speed in fixed point format 8|0
+  DARWIN_JOINT_SERVO1_SPEED        = 0x01F7, // speed in fixed point format 8|0
+  DARWIN_JOINT_SERVO2_SPEED        = 0x01F8, // speed in fixed point format 8|0
+  DARWIN_JOINT_SERVO3_SPEED        = 0x01F9, // speed in fixed point format 8|0
+  DARWIN_JOINT_SERVO4_SPEED        = 0x01FA, // speed in fixed point format 8|0
+  DARWIN_JOINT_SERVO5_SPEED        = 0x01FB, // speed in fixed point format 8|0
+  DARWIN_JOINT_SERVO6_SPEED        = 0x01FC, // speed in fixed point format 8|0
+  DARWIN_JOINT_SERVO7_SPEED        = 0x01FD, // speed in fixed point format 8|0
+  DARWIN_JOINT_SERVO8_SPEED        = 0x01FE, // speed in fixed point format 8|0
+  DARWIN_JOINT_SERVO9_SPEED        = 0x01FF, // speed in fixed point format 8|0
+  DARWIN_JOINT_SERVO10_SPEED       = 0x0200, // speed in fixed point format 8|0
+  DARWIN_JOINT_SERVO11_SPEED       = 0x0201, // speed in fixed point format 8|0
+  DARWIN_JOINT_SERVO12_SPEED       = 0x0202, // speed in fixed point format 8|0
+  DARWIN_JOINT_SERVO13_SPEED       = 0x0203, // speed in fixed point format 8|0
+  DARWIN_JOINT_SERVO14_SPEED       = 0x0204, // speed in fixed point format 8|0
+  DARWIN_JOINT_SERVO15_SPEED       = 0x0205, // speed in fixed point format 8|0
+  DARWIN_JOINT_SERVO16_SPEED       = 0x0206, // speed in fixed point format 8|0
+  DARWIN_JOINT_SERVO17_SPEED       = 0x0207, // speed in fixed point format 8|0
+  DARWIN_JOINT_SERVO18_SPEED       = 0x0208, // speed in fixed point format 8|0
+  DARWIN_JOINT_SERVO19_SPEED       = 0x0209, // speed in fixed point format 8|0
+  DARWIN_JOINT_SERVO20_SPEED       = 0x020A, // speed in fixed point format 8|0
+  DARWIN_JOINT_SERVO21_SPEED       = 0x020B, // speed in fixed point format 8|0
+  DARWIN_JOINT_SERVO22_SPEED       = 0x020C, // speed in fixed point format 8|0
+  DARWIN_JOINT_SERVO23_SPEED       = 0x020D, // speed in fixed point format 8|0
+  DARWIN_JOINT_SERVO24_SPEED       = 0x020E, // speed in fixed point format 8|0
+  DARWIN_JOINT_SERVO25_SPEED       = 0x020F, // speed in fixed point format 8|0
+  DARWIN_JOINT_SERVO26_SPEED       = 0x0210, // speed in fixed point format 8|0
+  DARWIN_JOINT_SERVO27_SPEED       = 0x0211, // speed in fixed point format 8|0
+  DARWIN_JOINT_SERVO28_SPEED       = 0x0212, // speed in fixed point format 8|0
+  DARWIN_JOINT_SERVO29_SPEED       = 0x0213, // speed in fixed point format 8|0
+  DARWIN_JOINT_SERVO30_SPEED       = 0x0214, // speed in fixed point format 8|0
+  DARWIN_JOINT_SERVO31_SPEED       = 0x0215, // speed in fixed point format 8|0
+  DARWIN_JOINT_SERVO0_ACCEL        = 0x0216, // acceleration in fixed point format 8|0 
+  DARWIN_JOINT_SERVO1_ACCEL        = 0x0217, // acceleration in fixed point format 8|0 
+  DARWIN_JOINT_SERVO2_ACCEL        = 0x0218, // acceleration in fixed point format 8|0 
+  DARWIN_JOINT_SERVO3_ACCEL        = 0x0219, // acceleration in fixed point format 8|0 
+  DARWIN_JOINT_SERVO4_ACCEL        = 0x021A, // acceleration in fixed point format 8|0 
+  DARWIN_JOINT_SERVO5_ACCEL        = 0x021B, // acceleration in fixed point format 8|0 
+  DARWIN_JOINT_SERVO6_ACCEL        = 0x021C, // acceleration in fixed point format 8|0 
+  DARWIN_JOINT_SERVO7_ACCEL        = 0x021D, // acceleration in fixed point format 8|0 
+  DARWIN_JOINT_SERVO8_ACCEL        = 0x021E, // acceleration in fixed point format 8|0 
+  DARWIN_JOINT_SERVO9_ACCEL        = 0x021F, // acceleration in fixed point format 8|0 
+  DARWIN_JOINT_SERVO10_ACCEL       = 0x0220, // acceleration in fixed point format 8|0 
+  DARWIN_JOINT_SERVO11_ACCEL       = 0x0221, // acceleration in fixed point format 8|0 
+  DARWIN_JOINT_SERVO12_ACCEL       = 0x0222, // acceleration in fixed point format 8|0 
+  DARWIN_JOINT_SERVO13_ACCEL       = 0x0223, // acceleration in fixed point format 8|0 
+  DARWIN_JOINT_SERVO14_ACCEL       = 0x0224, // acceleration in fixed point format 8|0 
+  DARWIN_JOINT_SERVO15_ACCEL       = 0x0225, // acceleration in fixed point format 8|0 
+  DARWIN_JOINT_SERVO16_ACCEL       = 0x0226, // acceleration in fixed point format 8|0 
+  DARWIN_JOINT_SERVO17_ACCEL       = 0x0227, // acceleration in fixed point format 8|0 
+  DARWIN_JOINT_SERVO18_ACCEL       = 0x0228, // acceleration in fixed point format 8|0 
+  DARWIN_JOINT_SERVO19_ACCEL       = 0x0229, // acceleration in fixed point format 8|0 
+  DARWIN_JOINT_SERVO20_ACCEL       = 0x022A, // acceleration in fixed point format 8|0 
+  DARWIN_JOINT_SERVO21_ACCEL       = 0x022B, // acceleration in fixed point format 8|0 
+  DARWIN_JOINT_SERVO22_ACCEL       = 0x022C, // acceleration in fixed point format 8|0 
+  DARWIN_JOINT_SERVO23_ACCEL       = 0x022D, // acceleration in fixed point format 8|0 
+  DARWIN_JOINT_SERVO24_ACCEL       = 0x022E, // acceleration in fixed point format 8|0 
+  DARWIN_JOINT_SERVO25_ACCEL       = 0x022F, // acceleration in fixed point format 8|0 
+  DARWIN_JOINT_SERVO26_ACCEL       = 0x0230, // acceleration in fixed point format 8|0 
+  DARWIN_JOINT_SERVO27_ACCEL       = 0x0231, // acceleration in fixed point format 8|0 
+  DARWIN_JOINT_SERVO28_ACCEL       = 0x0232, // acceleration in fixed point format 8|0 
+  DARWIN_JOINT_SERVO29_ACCEL       = 0x0233, // acceleration in fixed point format 8|0 
+  DARWIN_JOINT_SERVO30_ACCEL       = 0x0234, // acceleration in fixed point format 8|0 
+  DARWIN_JOINT_SERVO31_ACCEL       = 0x0235, // acceleration in fixed point format 8|0 
+  DARWIN_HEAD_CNTRL                = 0x0236, // bit 7 | bit 6 | bit 5 |  bit 4   | bit 3 | bit 2 |     bit 1     |      bit 0
+                                             //       |       |       | tracking |       |       | stop tracking | start tracking
+  DARWIN_HEAD_MAX_PAN_L            = 0x0237, // angle in fixed point format 9|7
+  DARWIN_HEAD_MAX_PAN_H            = 0x0238,
+  DARWIN_HEAD_MIN_PAN_L            = 0x0239, // angle in fixed point format 9|7
+  DARWIN_HEAD_MIN_PAN_H            = 0x023A,
+  DARWIN_HEAD_PAN_TARGET_L         = 0x023B, // angle in fixed point format 9|7
+  DARWIN_HEAD_PAN_TARGET_H         = 0x023C,
+  DARWIN_HEAD_MAX_TILT_L           = 0x023D, // angle in fixed point format 9|7
+  DARWIN_HEAD_MAX_TILT_H           = 0x023E,
+  DARWIN_HEAD_MIN_TILT_L           = 0x023F, // angle in fixed point format 9|7
+  DARWIN_HEAD_MIN_TILT_H           = 0x0240,
+  DARWIN_HEAD_TILT_TARGET_L        = 0x0241, // angle in fixed point format 9|7
+  DARWIN_HEAD_TILT_TARGET_H        = 0x0242
 } darwin_registers;
 
 #define      GPIO_BASE_ADDRESS       0x0100
@@ -405,6 +611,20 @@ typedef enum {
 #define      WALK_STATUS             0x10
 #define      WALK_PHASE              0xC0
 
+#define      JOINT_BASE_ADDRESS      0x01A2
+#define      JOINT_MEM_LENGTH        148
+#define      JOINT_START             0x01
+#define      JOINT_STOP              0x02
+#define      JOINT_STATUS            0x10
+
+#define      HEAD_BASE_ADDRESS       0x0236
+#define      HEAD_MEM_LENGTH         13
+#define      HEAD_EEPROM_ADDRESS     0x0044
+#define      HEAD_EEPROM_LENGTH      16
+#define      HEAD_START              0x01
+#define      HEAD_STOP               0x02
+#define      HEAD_STATUS             0x10
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/include/eeprom.h b/include/eeprom.h
index e0b5189c7d4359c097a839dbe341410ee6d71762..6ae5309ae699919c016e15447869d4d9707d2c9d 100755
--- a/include/eeprom.h
+++ b/include/eeprom.h
@@ -83,7 +83,7 @@ extern "C" {
 #define PAGE_FULL             ((uint8_t)0x80)
 
 /* Variables' number */
-#define NB_OF_VAR             ((uint8_t)0x44)
+#define NB_OF_VAR             ((uint8_t)0x54)
 
 /* Exported types ------------------------------------------------------------*/
 /* Exported macro ------------------------------------------------------------*/
diff --git a/include/eeprom_init.h b/include/eeprom_init.h
index 1917b78d07d036ff3168576ca92d7718d5bfd9bd..329b4ef37c86090a36cb3594c316f459ec0a88f6 100644
--- a/include/eeprom_init.h
+++ b/include/eeprom_init.h
@@ -65,6 +65,14 @@ extern "C" {
 #define    DEFAULT_WALK_ARM_SWING_GAIN      0x0030 // 1.5 in fixed point format 3 | 5
 #define    DEFAULT_WALK_MAX_VEL             0x0016 // 20 mm/s
 #define    DEFAULT_WALK_MAX_ROT_VEL         0x0008 // 8 degrees/s in fixed point format 5 | 3
+#define    DEFAULT_HEAD_PAN_P               0x028F // 0.01 in fixed point format 0|16 
+#define    DEFAULT_HEAD_PAN_I               0x0000 // 0.0 in fixed point format 0|16
+#define    DEFAULT_HEAD_PAN_D               0x0000 // 0.0 in fixed point format 0|16
+#define    DEFAULT_HEAD_PAN_I_CLAMP         0x0000 // 0.0 in fixed point format 9|7
+#define    DEFAULT_HEAD_TILT_P              0x028F // 0.01 in fixed point format 0|16
+#define    DEFAULT_HEAD_TILT_I              0x0000 // 0.0 in fixed point format 0|16
+#define    DEFAULT_HEAD_TILT_D              0x0000 // 0.0 in fixed point format 0|16
+#define    DEFAULT_HEAD_TILT_I_CLAMP        0x0000 // 0.0 in fixed point format 9|7
 
 
 #ifdef __cplusplus
diff --git a/include/head_tracking.h b/include/head_tracking.h
index ddba99bcf5aed0eb8862c827514aa58039072480..31d3a53c3c1a6bd0db61046305b257beef4ccee5 100644
--- a/include/head_tracking.h
+++ b/include/head_tracking.h
@@ -1,23 +1,41 @@
 #ifndef _HEAD_TRACKING_H
 #define _HEAD_TRACKINH_H
 
-#include "stm32f10x.h"
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "stm32f1xx.h"
+#include "motion_manager.h"
 
 // public functions
-void head_tracking_init(uint16_t period);
+void head_tracking_init(uint16_t period_us);
+inline void head_tracking_set_period(uint16_t period_us);
+inline uint16_t head_tracking_get_period(void);
 void head_tracking_start(void);
 void head_tracking_stop(void);
-void head_tracking_set_period(int16_t period);
-void head_tracking_set_pan_p(uint8_t value);
-void head_tracking_set_pan_i(uint8_t value);
-void head_tracking_set_pan_d(uint8_t value);
-void head_tracking_set_tilt_p(uint8_t value);
-void head_tracking_set_tilt_i(uint8_t value);
-void head_tracking_set_tilt_d(uint8_t value);
+uint8_t head_is_tracking(void);
+void head_tracking_set_pan_range(int16_t max_angle,int16_t min_angle);
+void head_tracking_get_pan_range(int16_t *max_angle,int16_t *min_angle);
+void head_tracking_set_pan_pid(uint16_t kp,uint16_t ki,uint16_t kd, uint16_t i_clamp);
+void head_tracking_get_pan_pid(uint16_t *kp,uint16_t *ki,uint16_t *kd, uint16_t *i_clamp);
+void head_tracking_set_tilt_range(int16_t max_angle,int16_t min_angle);
+void head_tracking_get_tilt_range(int16_t *max_angle,int16_t *min_angle);
+void head_tracking_set_tilt_pid(uint16_t kp,uint16_t ki,uint16_t kd, uint16_t i_clamp);
+void head_tracking_get_tilt_pid(uint16_t *kp,uint16_t *ki,uint16_t *kd, uint16_t *i_clamp);
 void head_tracking_set_target_pan(int16_t target);
 void head_tracking_set_target_tilt(int16_t target);
 
+// operation functions
+uint8_t head_tracking_in_range(unsigned short int address, unsigned short int length);
+void head_tracking_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
+void head_tracking_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
+
 // motion manager process function
 void head_tracking_process(void);
 
+#ifdef __cplusplus
+}
+#endif
+
 #endif
diff --git a/include/imu.h b/include/imu.h
index 0a72629d94a0214c03a2cabc1aec74e3ec4ece91..079304bf1b235660acb081ab1186a926366633c0 100755
--- a/include/imu.h
+++ b/include/imu.h
@@ -14,6 +14,7 @@ void imu_stop(void);
 void imu_set_calibration_samples(uint16_t num_samples);
 void imu_start_gyro_cal(void);
 void imu_get_gyro_data(int32_t *x, int32_t *y, int32_t *z);
+uint8_t imu_is_gyro_calibrated(void);
 // operation functions
 uint8_t imu_in_range(unsigned short int address,unsigned short int length);
 void imu_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
diff --git a/include/joint_motion.h b/include/joint_motion.h
index 654f9d9df99b2592984781b34de134550fa21828..70587b38aa8d0caad2dc7aaf26318d7360ee1525 100644
--- a/include/joint_motion.h
+++ b/include/joint_motion.h
@@ -1,20 +1,34 @@
 #ifndef _JOINT_MOTION_H
 #define _JOINT_MOTION_H
 
-#include "stm32f10x.h"
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "stm32f1xx.h"
+#include "motion_manager.h"
+
+#define NUM_JOINT_GROUPS 4
 
 // public functions
-void joint_motion_init(uint16_t period);
-void joint_motion_set_period(uint16_t period);
-uint16_t joint_motion_get_period(void);
-inline void joint_motion_load_target_angle(uint8_t servo_id, int16_t angle);
-inline void joint_motion_load_target_speed(uint8_t servo_id, uint8_t speed);
-inline void joint_motion_load_target_accel(uint8_t servo_id, uint8_t accel);
-void joint_motion_start(void);
-void joint_motion_stop(void);
-uint8_t are_joints_moving(void);
+void joint_motion_init(uint16_t period_us);
+inline void joint_motion_set_period(uint16_t period_us);
+inline uint16_t joint_motion_get_period(void);
+void joint_motion_set_group_servos(uint8_t group_id,uint32_t servos);
+uint32_t joint_motion_get_group_servos(uint8_t group_id);
+void joint_motion_start(uint8_t group_id);
+void joint_motion_stop(uint8_t group_id);
+uint8_t are_joints_moving(uint8_t group_id);
 
+// operation functions
+uint8_t joint_motion_in_range(unsigned short int address, unsigned short int length);
+void joint_motion_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
+void joint_motion_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
 // motion manager interface functions
 void joint_motion_process(void);
 
+#ifdef __cplusplus
+}
+#endif
+
 #endif
diff --git a/include/motion_manager.h b/include/motion_manager.h
index 04caa19e1c496f5fe60de50f61349fdbeec819ce..c3987f5af395e597c677889d9083b498dc207529 100755
--- a/include/motion_manager.h
+++ b/include/motion_manager.h
@@ -33,7 +33,8 @@ typedef enum {
 typedef enum {MM_NONE          = 0,
               MM_ACTION        = 1,
               MM_WALKING       = 2,
-              MM_JOINTS        = 3} TModules;
+              MM_JOINTS        = 3,
+              MM_HEAD          = 4} TModules;
 
 typedef struct
 {
diff --git a/include/ram.h b/include/ram.h
index b096a64789613b6a9e627e74b5095a24768341d6..d920367d293c06bb7aa4e8a4ce2fe2cd1a47bd0b 100755
--- a/include/ram.h
+++ b/include/ram.h
@@ -14,7 +14,7 @@ extern "C" {
 #define      RAM_BAD_BIT      -3
 #define      RAM_BAD_ACCESS   -4
 
-#define      RAM_SIZE          512
+#define      RAM_SIZE          1024
 
 extern uint8_t ram_data[RAM_SIZE];
 
diff --git a/src/darwin_dyn_slave.c b/src/darwin_dyn_slave.c
index 2970d520007dd8c45459d4fa3b01aa7f586e8ffd..952c846091e9a0eda5826d57628796939e5416a5 100755
--- a/src/darwin_dyn_slave.c
+++ b/src/darwin_dyn_slave.c
@@ -9,6 +9,8 @@
 #include "motion_manager.h"
 #include "action.h"
 #include "walking.h"
+#include "joint_motion.h"
+#include "head_tracking.h"
 
 /* timer for the execution of the dynamixel slave loop */
 #define     DYN_SLAVE_TIMER                   TIM7
@@ -80,6 +82,12 @@ unsigned char darwin_on_write(unsigned short int address,unsigned short int leng
   // walk commands
   if(walking_in_range(address,length))
     walking_process_write_cmd(address,length,data);
+  // joint motion commands
+  if(joint_motion_in_range(address,length))
+    joint_motion_process_write_cmd(address,length,data);
+  // head_tracking commands
+  if(head_tracking_in_range(address,length))
+    head_tracking_process_write_cmd(address,length,data);
   // write eeprom
   for(i=address,j=0;i<LAST_EEPROM_OFFSET && i<(address+length);i++,j++)
     EE_WriteVariable(i,data[j]);
diff --git a/src/eeprom.c b/src/eeprom.c
index 20b740e3926f02b19f8478d5f67e4372c761d885..701a3ad081d42f9a51233fbb249ead6ff147ced6 100755
--- a/src/eeprom.c
+++ b/src/eeprom.c
@@ -121,7 +121,24 @@ uint16_t eeprom_data[] __attribute__ ((section (".eeprom")))={VALID_PAGE,
                                                               DEFAULT_WALK_PELVIS_OFFSET,        WALK_PELVIS_OFFSET,
                                                               DEFAULT_WALK_ARM_SWING_GAIN,       WALK_ARM_SWING_GAIN,
                                                               DEFAULT_WALK_MAX_VEL,              WALK_MAX_VEL,
-                                                              DEFAULT_WALK_MAX_ROT_VEL,          WALK_MAX_ROT_VEL};
+                                                              DEFAULT_WALK_MAX_ROT_VEL,          WALK_MAX_ROT_VEL,
+                                                              DEFAULT_HEAD_PAN_P&0xFF,           HEAD_PAN_P,
+                                                              DEFAULT_HEAD_PAN_P>>8,             HEAD_PAN_P+1,
+                                                              DEFAULT_HEAD_PAN_I&0xFF,           HEAD_PAN_I,
+                                                              DEFAULT_HEAD_PAN_I>>8,             HEAD_PAN_I+1,
+                                                              DEFAULT_HEAD_PAN_D&0xFF,           HEAD_PAN_D,
+                                                              DEFAULT_HEAD_PAN_D>>8,             HEAD_PAN_D+1,
+                                                              DEFAULT_HEAD_PAN_I_CLAMP&0xFF,     HEAD_PAN_I_CLAMP,
+                                                              DEFAULT_HEAD_PAN_I_CLAMP>>8,       HEAD_PAN_I_CLAMP+1,
+                                                              DEFAULT_HEAD_PAN_D>>8,             HEAD_PAN_D+1,
+                                                              DEFAULT_HEAD_TILT_P&0xFF,          HEAD_TILT_P,
+                                                              DEFAULT_HEAD_TILT_P>>8,            HEAD_TILT_P+1,
+                                                              DEFAULT_HEAD_TILT_I&0xFF,          HEAD_TILT_I,
+                                                              DEFAULT_HEAD_TILT_I>>8,            HEAD_TILT_I+1,
+                                                              DEFAULT_HEAD_TILT_D&0xFF,          HEAD_TILT_D,
+                                                              DEFAULT_HEAD_TILT_D>>8,            HEAD_TILT_D+1,
+                                                              DEFAULT_HEAD_TILT_I_CLAMP&0xFF,    HEAD_TILT_I_CLAMP,
+                                                              DEFAULT_HEAD_TILT_I_CLAMP>>8,      HEAD_TILT_I_CLAMP+1};
 
 /* Private function prototypes -----------------------------------------------*/
 /* Private functions ---------------------------------------------------------*/
diff --git a/src/head_tracking.c b/src/head_tracking.c
index 8acf565fc1e0bb66b9485d1fc3a7116e0228f105..d996ad9ec8c7454d70a1e3334de59af01cf3d260 100644
--- a/src/head_tracking.c
+++ b/src/head_tracking.c
@@ -1,57 +1,71 @@
 #include "head_tracking.h"
 #include "ram.h"
-#include "motion_manager.h"
-
-#define DEFAULT_PAN_P    40//
-#define DEFAULT_PAN_I    0//
-#define DEFAULT_PAN_D    0//
-
-#define DEFAULT_TILT_P   40//
-#define DEFAULT_TILT_I   0//
-#define DEFAULT_TILT_D   0//
 
 // private variables
-int64_t prev_error_pan;// 48|16
-int64_t prev_error_tilt;// 48|16
-int64_t integral_pan;
-int64_t integral_tilt;
-uint8_t pan_p;
-uint8_t pan_i;
-uint8_t pan_d;
-uint8_t tilt_p;
-uint8_t tilt_i;
-uint8_t tilt_d;
-int64_t pan_target;// 48|16
-int64_t tilt_target;// 48|16
+typedef struct{
+  uint16_t kp;
+  uint16_t ki;
+  uint16_t kd;
+  int64_t prev_error;// 48|16
+  int64_t integral_part;
+  int64_t integral_clamp;
+  int64_t max_angle;
+  int64_t min_angle;
+  int64_t target_angle;
+}TJointControl;
+
+TJointControl head_tracking_pan;
+TJointControl head_tracking_tilt;
 int64_t head_tracking_period;
+int16_t head_tracking_period_us;
 uint8_t head_tracking_enabled;
 
 // public functions
-void head_tracking_init(uint16_t period)
+void head_tracking_init(uint16_t period_us)
 {
   /* initialize private variables */
-  prev_error_pan=0;
-  prev_error_tilt=0;
-  integral_pan=0;
-  integral_tilt=0;
-  pan_p=DEFAULT_PAN_P;
-  ram_write_byte(DARWIN_HEAD_PAN_P,DEFAULT_PAN_P);
-  pan_i=DEFAULT_PAN_I;
-  ram_write_byte(DARWIN_HEAD_PAN_I,DEFAULT_PAN_I);
-  pan_d=DEFAULT_PAN_D;
-  ram_write_byte(DARWIN_HEAD_PAN_D,DEFAULT_PAN_D);
-  tilt_p=DEFAULT_TILT_P;
-  ram_write_byte(DARWIN_HEAD_TILT_P,DEFAULT_TILT_P);
-  tilt_i=DEFAULT_TILT_I;
-  ram_write_byte(DARWIN_HEAD_TILT_I,DEFAULT_TILT_I);
-  tilt_d=DEFAULT_TILT_D;
-  ram_write_byte(DARWIN_HEAD_TILT_D,DEFAULT_TILT_D);
-  pan_target=0;
-  tilt_target=0;
-  head_tracking_period=period;
+  head_tracking_pan.kp=ram_data[DARWIN_HEAD_PAN_P_L]+(ram_data[DARWIN_HEAD_PAN_P_H]<<8);
+  head_tracking_pan.ki=ram_data[DARWIN_HEAD_PAN_I_L]+(ram_data[DARWIN_HEAD_PAN_I_H]<<8);
+  head_tracking_pan.kd=ram_data[DARWIN_HEAD_PAN_D_L]+(ram_data[DARWIN_HEAD_PAN_D_H]<<8);
+  head_tracking_pan.prev_error=0;
+  head_tracking_pan.integral_part=0;
+  head_tracking_pan.integral_clamp=(ram_data[DARWIN_HEAD_PAN_I_CLAMP_L]+(ram_data[DARWIN_HEAD_PAN_I_CLAMP_H]<<8))<<9;
+  head_tracking_pan.min_angle=manager_get_cw_angle_limit(L_PAN)<<9;
+  ram_data[DARWIN_HEAD_MIN_PAN_L]=manager_get_cw_angle_limit(L_PAN)%256;
+  ram_data[DARWIN_HEAD_MIN_PAN_H]=manager_get_cw_angle_limit(L_PAN)/256;
+  head_tracking_pan.max_angle=manager_get_ccw_angle_limit(L_PAN)<<9;
+  ram_data[DARWIN_HEAD_MAX_PAN_L]=manager_get_ccw_angle_limit(L_PAN)%256;
+  ram_data[DARWIN_HEAD_MAX_PAN_H]=manager_get_ccw_angle_limit(L_PAN)/256;
+  head_tracking_pan.target_angle=0;
+  head_tracking_tilt.kp=ram_data[DARWIN_HEAD_TILT_P_L]+(ram_data[DARWIN_HEAD_TILT_P_H]<<8);
+  head_tracking_tilt.ki=ram_data[DARWIN_HEAD_TILT_I_L]+(ram_data[DARWIN_HEAD_TILT_I_H]<<8);
+  head_tracking_tilt.kd=ram_data[DARWIN_HEAD_TILT_D_L]+(ram_data[DARWIN_HEAD_TILT_D_H]<<8);
+  head_tracking_tilt.prev_error=0;
+  head_tracking_tilt.integral_part=0;
+  head_tracking_tilt.integral_clamp=(ram_data[DARWIN_HEAD_TILT_I_CLAMP_L]+(ram_data[DARWIN_HEAD_TILT_I_CLAMP_H]<<8))<<9;
+  head_tracking_tilt.min_angle=manager_get_cw_angle_limit(L_TILT)<<9;
+  ram_data[DARWIN_HEAD_MIN_TILT_L]=manager_get_cw_angle_limit(L_TILT)%256;
+  ram_data[DARWIN_HEAD_MIN_TILT_H]=manager_get_cw_angle_limit(L_TILT)/256;
+  head_tracking_tilt.max_angle=manager_get_ccw_angle_limit(L_TILT)<<9;
+  ram_data[DARWIN_HEAD_MAX_TILT_L]=manager_get_ccw_angle_limit(L_TILT)%256;
+  ram_data[DARWIN_HEAD_MAX_TILT_H]=manager_get_ccw_angle_limit(L_TILT)/256;
+  head_tracking_tilt.target_angle=0;
+  head_tracking_period=(period_us<<16)/1000000;
+  head_tracking_period_us=period_us;
   head_tracking_enabled=0x00;
 }
 
+inline void head_tracking_set_period(uint16_t period_us)
+{
+  head_tracking_period=(period_us<<16)/1000000;
+  head_tracking_period_us=period_us;
+}
+
+inline uint16_t head_tracking_get_period(void)
+{
+  return head_tracking_period_us;
+}
+
 void head_tracking_start(void)
 {
   head_tracking_enabled=0x01;
@@ -62,57 +76,191 @@ void head_tracking_stop(void)
   head_tracking_enabled=0x00;
 }
 
-void head_tracking_set_period(int16_t period)
+uint8_t head_is_tracking(void)
 {
-  head_tracking_period=period;
+  return head_tracking_enabled;
 }
 
-void head_tracking_set_pan_p(uint8_t value)
+void head_tracking_set_pan_range(int16_t max_angle,int16_t min_angle)
 {
-  pan_p=value;
-  ram_write_byte(DARWIN_HEAD_PAN_P,value);
+  head_tracking_pan.max_angle=max_angle<<9;
+  ram_data[DARWIN_HEAD_MAX_PAN_L]=max_angle%256;
+  ram_data[DARWIN_HEAD_MAX_PAN_H]=max_angle/256;
+  head_tracking_pan.min_angle=min_angle<<9;
+  ram_data[DARWIN_HEAD_MIN_PAN_L]=min_angle%256;
+  ram_data[DARWIN_HEAD_MIN_PAN_H]=min_angle/256;
 }
 
-void head_tracking_set_pan_i(uint8_t value)
+void head_tracking_get_pan_range(int16_t *max_angle,int16_t *min_angle)
 {
-  pan_i=value;
-  ram_write_byte(DARWIN_HEAD_PAN_I,value);
+  *max_angle=head_tracking_pan.max_angle>>9;
+  *min_angle=head_tracking_pan.min_angle>>9;
 }
 
-void head_tracking_set_pan_d(uint8_t value)
+void head_tracking_set_pan_pid(uint16_t kp,uint16_t ki,uint16_t kd, uint16_t i_clamp)
 {
-  pan_d=value;
-  ram_write_byte(DARWIN_HEAD_PAN_D,value);
+  head_tracking_pan.kp=kp;
+  ram_data[DARWIN_HEAD_PAN_P_L]=kp%256;
+  ram_data[DARWIN_HEAD_PAN_P_H]=kp/256;
+  head_tracking_pan.ki=ki;
+  ram_data[DARWIN_HEAD_PAN_I_L]=ki%256;
+  ram_data[DARWIN_HEAD_PAN_I_H]=ki/256;
+  head_tracking_pan.kd=kd;
+  ram_data[DARWIN_HEAD_PAN_D_L]=kd%256;
+  ram_data[DARWIN_HEAD_PAN_D_H]=kd/256;
+  head_tracking_pan.integral_clamp=i_clamp<<9;
+  ram_data[DARWIN_HEAD_PAN_I_CLAMP_L]=i_clamp%256;
+  ram_data[DARWIN_HEAD_PAN_I_CLAMP_H]=i_clamp/256;
 }
 
-void head_tracking_set_tilt_p(uint8_t value)
+void head_tracking_get_pan_pid(uint16_t *kp,uint16_t *ki,uint16_t *kd, uint16_t *i_clamp)
 {
-  tilt_p=value;
-  ram_write_byte(DARWIN_HEAD_TILT_P,value);
+  *kp=head_tracking_pan.kp;
+  *ki=head_tracking_pan.ki;
+  *kd=head_tracking_pan.kd;
+  *i_clamp=head_tracking_pan.integral_clamp>>9;
 }
 
-void head_tracking_set_tilt_i(uint8_t value)
+void head_tracking_set_tilt_range(int16_t max_angle,int16_t min_angle)
 {
-  tilt_i=value;
-  ram_write_byte(DARWIN_HEAD_TILT_I,value);
+  head_tracking_tilt.max_angle=max_angle<<9;
+  ram_data[DARWIN_HEAD_MAX_TILT_L]=max_angle%256;
+  ram_data[DARWIN_HEAD_MAX_TILT_H]=max_angle/256;
+  head_tracking_tilt.min_angle=min_angle<<9;
+  ram_data[DARWIN_HEAD_MIN_TILT_L]=min_angle%256;
+  ram_data[DARWIN_HEAD_MIN_TILT_H]=min_angle/256;
 }
 
-void head_tracking_set_tilt_d(uint8_t value)
+void head_tracking_get_tilt_range(int16_t *max_angle,int16_t *min_angle)
 {
-  tilt_d=value;
-  ram_write_byte(DARWIN_HEAD_TILT_D,value);
+  *max_angle=head_tracking_tilt.max_angle>>9;
+  *min_angle=head_tracking_tilt.min_angle>>9;
+}
+
+void head_tracking_set_tilt_pid(uint16_t kp,uint16_t ki,uint16_t kd, uint16_t i_clamp)
+{
+  head_tracking_tilt.kp=kp;
+  ram_data[DARWIN_HEAD_TILT_P_L]=kp%256;
+  ram_data[DARWIN_HEAD_TILT_P_H]=kp/256;
+  head_tracking_tilt.ki=ki;
+  ram_data[DARWIN_HEAD_TILT_I_L]=ki%256;
+  ram_data[DARWIN_HEAD_TILT_I_H]=ki/256;
+  head_tracking_tilt.kd=kd;
+  ram_data[DARWIN_HEAD_TILT_D_L]=kd%256;
+  ram_data[DARWIN_HEAD_TILT_D_H]=kd/256;
+  head_tracking_tilt.integral_clamp=i_clamp<<9;
+  ram_data[DARWIN_HEAD_TILT_I_CLAMP_L]=i_clamp%256;
+  ram_data[DARWIN_HEAD_TILT_I_CLAMP_H]=i_clamp/256;
+}
+
+void head_tracking_get_tilt_pid(uint16_t *kp,uint16_t *ki,uint16_t *kd, uint16_t *i_clamp)
+{
+  *kp=head_tracking_tilt.kp;
+  *ki=head_tracking_tilt.ki;
+  *kd=head_tracking_tilt.kd;
+  *i_clamp=head_tracking_tilt.integral_clamp>>9;
 }
 
 void head_tracking_set_target_pan(int16_t target)
 {
-  pan_target=target<<9;
-  ram_write_word(DARWIN_HEAD_PAN_L,target);
+  head_tracking_pan.target_angle=target<<9;
+  ram_data[DARWIN_HEAD_PAN_TARGET_L]=target%256;
+  ram_data[DARWIN_HEAD_PAN_TARGET_H]=target/256;
 }
 
 void head_tracking_set_target_tilt(int16_t target)
 {
-  tilt_target=target<<9;
-  ram_write_word(DARWIN_HEAD_TILT_L,target);
+  head_tracking_tilt.target_angle=target<<9;
+  ram_data[DARWIN_HEAD_TILT_TARGET_L]=target%256;
+  ram_data[DARWIN_HEAD_TILT_TARGET_H]=target/256;
+}
+
+// operation functions
+uint8_t head_tracking_in_range(unsigned short int address, unsigned short int length)
+{
+  if(ram_in_window(HEAD_BASE_ADDRESS,HEAD_MEM_LENGTH,address,length) ||
+     ram_in_window(HEAD_EEPROM_ADDRESS,HEAD_EEPROM_LENGTH,address,length))
+    return 0x01;
+  else
+    return 0x00;
+}
+
+void head_tracking_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
+{
+
+}
+
+void head_tracking_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
+{
+  uint16_t kp,kd,ki,i_clamp;
+  int16_t max,min,target;
+
+  // head tracking control
+  if(ram_in_range(DARWIN_HEAD_CNTRL,address,length))
+  {
+    if(data[DARWIN_HEAD_CNTRL-address]&HEAD_START)
+      head_tracking_start();
+    if(data[DARWIN_HEAD_CNTRL-address]&HEAD_STOP)
+      head_tracking_stop();
+  }
+  if(ram_in_window(DARWIN_HEAD_PAN_P_L,8,address,length))
+  {
+    head_tracking_get_pan_pid(&kp,&kd,&ki,&i_clamp);
+    if(ram_in_range(DARWIN_HEAD_PAN_P_L,address,length)) kp=(kp&0xFF00)|data[DARWIN_HEAD_PAN_P_L-address];
+    if(ram_in_range(DARWIN_HEAD_PAN_P_H,address,length)) kp=(kp&0x00FF)|(data[DARWIN_HEAD_PAN_P_H-address]<<8);
+    if(ram_in_range(DARWIN_HEAD_PAN_I_L,address,length)) ki=(ki&0xFF00)|data[DARWIN_HEAD_PAN_I_L-address];
+    if(ram_in_range(DARWIN_HEAD_PAN_I_H,address,length)) ki=(ki&0x00FF)|(data[DARWIN_HEAD_PAN_I_H-address]<<8);
+    if(ram_in_range(DARWIN_HEAD_PAN_D_L,address,length)) kd=(kd&0xFF00)|data[DARWIN_HEAD_PAN_D_L-address];
+    if(ram_in_range(DARWIN_HEAD_PAN_D_H,address,length)) kd=(kd&0x00FF)|(data[DARWIN_HEAD_PAN_D_H-address]<<8);
+    if(ram_in_range(DARWIN_HEAD_PAN_I_CLAMP_L,address,length)) i_clamp=(i_clamp&0xFF00)|data[DARWIN_HEAD_PAN_I_CLAMP_L-address];
+    if(ram_in_range(DARWIN_HEAD_PAN_I_CLAMP_H,address,length)) i_clamp=(i_clamp&0x00FF)|(data[DARWIN_HEAD_PAN_I_CLAMP_H-address]<<8);
+    head_tracking_set_pan_pid(kp,kd,ki,i_clamp);
+  }
+  if(ram_in_window(DARWIN_HEAD_TILT_P_L,8,address,length))
+  {
+    head_tracking_get_tilt_pid(&kp,&kd,&ki,&i_clamp);
+    if(ram_in_range(DARWIN_HEAD_TILT_P_L,address,length)) kp=(kp&0xFF00)|data[DARWIN_HEAD_TILT_P_L-address];
+    if(ram_in_range(DARWIN_HEAD_TILT_P_H,address,length)) kp=(kp&0x00FF)|(data[DARWIN_HEAD_TILT_P_H-address]<<8);
+    if(ram_in_range(DARWIN_HEAD_TILT_I_L,address,length)) ki=(ki&0xFF00)|data[DARWIN_HEAD_TILT_I_L-address];
+    if(ram_in_range(DARWIN_HEAD_TILT_I_H,address,length)) ki=(ki&0x00FF)|(data[DARWIN_HEAD_TILT_I_H-address]<<8);
+    if(ram_in_range(DARWIN_HEAD_TILT_D_L,address,length)) kd=(kd&0xFF00)|data[DARWIN_HEAD_TILT_D_L-address];
+    if(ram_in_range(DARWIN_HEAD_TILT_D_H,address,length)) kd=(kd&0x00FF)|(data[DARWIN_HEAD_TILT_D_H-address]<<8);
+    if(ram_in_range(DARWIN_HEAD_TILT_I_CLAMP_L,address,length)) i_clamp=(i_clamp&0xFF00)|data[DARWIN_HEAD_TILT_I_CLAMP_L-address];
+    if(ram_in_range(DARWIN_HEAD_TILT_I_CLAMP_H,address,length)) i_clamp=(i_clamp&0x00FF)|(data[DARWIN_HEAD_TILT_I_CLAMP_H-address]<<8);
+    head_tracking_set_tilt_pid(kp,kd,ki,i_clamp);
+  }
+  if(ram_in_window(DARWIN_HEAD_MAX_PAN_L,4,address,length))
+  {
+    head_tracking_get_pan_range(&max,&min);
+    if(ram_in_range(DARWIN_HEAD_MAX_PAN_L,address,length)) max=(max&0xFF00)|data[DARWIN_HEAD_MAX_PAN_L-address];
+    if(ram_in_range(DARWIN_HEAD_MAX_PAN_H,address,length)) max=(max&0x00FF)|(data[DARWIN_HEAD_MAX_PAN_H-address]<<8);
+    if(ram_in_range(DARWIN_HEAD_MIN_PAN_L,address,length)) min=(min&0xFF00)|data[DARWIN_HEAD_MIN_PAN_L-address];
+    if(ram_in_range(DARWIN_HEAD_MIN_PAN_H,address,length)) min=(min&0x00FF)|(data[DARWIN_HEAD_MIN_PAN_H-address]<<8);
+    head_tracking_set_pan_range(max,min);
+  }
+  if(ram_in_window(DARWIN_HEAD_MAX_TILT_L,4,address,length))
+  {
+    head_tracking_get_tilt_range(&max,&min);
+    if(ram_in_range(DARWIN_HEAD_MAX_TILT_L,address,length)) max=(max&0xFF00)|data[DARWIN_HEAD_MAX_TILT_L-address];
+    if(ram_in_range(DARWIN_HEAD_MAX_TILT_H,address,length)) max=(max&0x00FF)|(data[DARWIN_HEAD_MAX_TILT_H-address]<<8);
+    if(ram_in_range(DARWIN_HEAD_MIN_TILT_L,address,length)) min=(min&0xFF00)|data[DARWIN_HEAD_MIN_TILT_L-address];
+    if(ram_in_range(DARWIN_HEAD_MIN_TILT_H,address,length)) min=(min&0x00FF)|(data[DARWIN_HEAD_MIN_TILT_H-address]<<8);
+    head_tracking_set_tilt_range(max,min);
+  }
+  if(ram_in_window(DARWIN_HEAD_PAN_TARGET_L,2,address,length))
+  {
+    target=head_tracking_pan.target_angle;
+    if(ram_in_range(DARWIN_HEAD_PAN_TARGET_L,address,length)) target=(target&0xFF00)|data[DARWIN_HEAD_PAN_TARGET_L-address];
+    if(ram_in_range(DARWIN_HEAD_PAN_TARGET_H,address,length)) target=(target&0x00FF)|(data[DARWIN_HEAD_PAN_TARGET_H-address]<<8);
+    head_tracking_pan.target_angle=target;
+  }
+  if(ram_in_window(DARWIN_HEAD_TILT_TARGET_L,2,address,length))
+  {
+    target=head_tracking_tilt.target_angle;
+    if(ram_in_range(DARWIN_HEAD_TILT_TARGET_L,address,length)) target=(target&0xFF00)|data[DARWIN_HEAD_TILT_TARGET_L-address];
+    if(ram_in_range(DARWIN_HEAD_TILT_TARGET_H,address,length)) target=(target&0x00FF)|(data[DARWIN_HEAD_TILT_TARGET_H-address]<<8);
+    head_tracking_tilt.target_angle=target;
+  }
 }
 
 // motion manager process function
@@ -123,25 +271,45 @@ void head_tracking_process(void)
 
   if(head_tracking_enabled)
   {
-    ram_set_bit(DARWIN_HEAD_STATUS,0x01);
-    if(manager_get_module(HEAD_PAN)==MM_HEAD)
+    ram_data[DARWIN_HEAD_CNTRL]|=HEAD_STATUS;
+    if(manager_get_module(L_PAN)==MM_HEAD)
     {
-      pan_error = pan_target-current_angles[HEAD_PAN];
-      integral_pan += (pan_error*head_tracking_period)>>16;
-      derivative_pan = ((pan_error - prev_error_pan)<<16)/head_tracking_period;
-      current_angles[HEAD_PAN]+=(pan_p*pan_error+pan_i*integral_pan+pan_d*derivative_pan)>>10;
-      prev_error_pan = pan_error;
+      pan_error = head_tracking_pan.target_angle-manager_current_angles[L_PAN];
+      head_tracking_pan.integral_part+=(pan_error*head_tracking_period)>>16;
+      if(head_tracking_pan.integral_part>head_tracking_pan.integral_clamp)
+        head_tracking_pan.integral_part=head_tracking_pan.integral_clamp;
+      else if(head_tracking_pan.integral_part<-head_tracking_pan.integral_clamp)
+        head_tracking_pan.integral_part=-head_tracking_pan.integral_clamp;
+      derivative_pan = ((pan_error - head_tracking_pan.prev_error)<<16)/head_tracking_period;
+      manager_current_angles[L_PAN]+=((head_tracking_pan.kp*pan_error)>>16);
+      manager_current_angles[L_PAN]+=((head_tracking_pan.ki*head_tracking_pan.integral_part)>>16);
+      manager_current_angles[L_PAN]+=((head_tracking_pan.kd*derivative_pan)>>16);
+      if(manager_current_angles[L_PAN]>head_tracking_pan.max_angle)
+        manager_current_angles[L_PAN]=head_tracking_pan.max_angle;
+      else if(manager_current_angles[L_PAN]<head_tracking_pan.min_angle)
+        manager_current_angles[L_PAN]=head_tracking_pan.min_angle;
+      head_tracking_pan.prev_error = pan_error;
     }
-    if(manager_get_module(HEAD_TILT)==MM_HEAD)
+    if(manager_get_module(L_TILT)==MM_HEAD)
     { 
-      tilt_error = tilt_target-current_angles[HEAD_TILT];
-      integral_tilt += (tilt_error*head_tracking_period)>>16;
-      derivative_tilt = ((tilt_error - prev_error_tilt)<<16)/head_tracking_period;
-      current_angles[HEAD_TILT]+=(tilt_p*tilt_error+tilt_i*integral_tilt+tilt_d*derivative_tilt)>>10;
-      prev_error_tilt = tilt_error;
+      tilt_error = head_tracking_tilt.target_angle-manager_current_angles[L_TILT];
+      head_tracking_tilt.integral_part+=(tilt_error*head_tracking_period)>>16;
+      if(head_tracking_tilt.integral_part>head_tracking_tilt.integral_clamp)
+        head_tracking_tilt.integral_part=head_tracking_tilt.integral_clamp;
+      else if(head_tracking_tilt.integral_part<-head_tracking_tilt.integral_clamp)
+        head_tracking_tilt.integral_part=-head_tracking_tilt.integral_clamp;
+      derivative_tilt = ((tilt_error - head_tracking_tilt.prev_error)<<16)/head_tracking_period;
+      manager_current_angles[L_TILT]+=((head_tracking_tilt.kp*tilt_error)>>16);
+      manager_current_angles[L_TILT]+=((head_tracking_tilt.ki*head_tracking_tilt.integral_part)>>16);
+      manager_current_angles[L_TILT]+=((head_tracking_tilt.kd*derivative_tilt)>>16);
+      if(manager_current_angles[L_TILT]>head_tracking_tilt.max_angle)
+        manager_current_angles[L_TILT]=head_tracking_tilt.max_angle;
+      else if(manager_current_angles[L_TILT]<head_tracking_tilt.min_angle)
+        manager_current_angles[L_TILT]=head_tracking_tilt.min_angle;
+      head_tracking_tilt.prev_error = tilt_error;
     }
   }
   else
-    ram_clear_bit(DARWIN_HEAD_STATUS,0x01);
+    ram_data[DARWIN_HEAD_CNTRL]&=(~HEAD_STATUS);
 }
 
diff --git a/src/imu.c b/src/imu.c
index 8175e80efb25fd04a7ac6755ff812c12c918ae06..3d3f0478946591baac10d9cfa0017ce762f82be9 100755
--- a/src/imu.c
+++ b/src/imu.c
@@ -588,6 +588,14 @@ void imu_start_gyro_cal(void)
   gyro_calibration=0x01;
 }
 
+uint8_t imu_is_gyro_calibrated(void)
+{
+  if(ram_data[DARWIN_IMU_CNTRL])
+    return 0x00;
+  else
+    return 0x01;
+}
+
 void imu_get_gyro_data(int32_t *x, int32_t *y, int32_t *z)
 {
   *x=gyro_data[0];
diff --git a/src/joint_motion.c b/src/joint_motion.c
index 0672858f8856f35ff03948b60824ac0433f4c637..e1d92efdcc78be09b72e108766bc7e5bf48cbf46 100644
--- a/src/joint_motion.c
+++ b/src/joint_motion.c
@@ -1,7 +1,5 @@
 #include "joint_motion.h"
-#include "motion_manager.h"
 #include "ram.h"
-#include "gpio.h"
 #include <stdlib.h>
 
 // private variables
@@ -14,12 +12,15 @@ int64_t joint_current_speeds[MANAGER_MAX_NUM_SERVOS];//48|16
 
 int8_t joint_dir[MANAGER_MAX_NUM_SERVOS];// joint_direction
 
-uint8_t joint_stop;
-uint8_t joint_moving;
 int64_t joint_motion_period;
+int16_t joint_motion_period_us;
+
+uint8_t joint_stop[NUM_JOINT_GROUPS];
+uint8_t joint_moving[NUM_JOINT_GROUPS];
+uint32_t joint_group_servos[NUM_JOINT_GROUPS];
 
 // public functions
-void joint_motion_init(uint16_t period)
+void joint_motion_init(uint16_t period_us)
 {
   uint8_t i;
 
@@ -27,153 +28,243 @@ void joint_motion_init(uint16_t period)
   for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
   {
     // target values
-    joint_target_angles[i]=current_angles[i];
+    joint_target_angles[i]=manager_current_angles[i];
     joint_target_speeds[i]=0;
     joint_target_accels[i]=0;
     // current values
-    joint_current_angles[i]=current_angles[i];
+    joint_current_angles[i]=manager_current_angles[i];
     joint_current_speeds[i]=0;
     // the current angles, speeds and accelerations are in RAM
     joint_dir[i]=0;
   }
-  joint_stop=0x00;
-  joint_motion_period=period;
-}
-
-void joint_motion_set_period(uint16_t period)
-{
-  joint_motion_period=period;
+  for(i=0;i<NUM_JOINT_GROUPS;i++)
+  {
+    joint_stop[i]=0x00;
+    joint_moving[i]=0x00;
+    joint_group_servos[i]=0x00000000;
+  }
+  joint_motion_period=(period_us<<16)/1000000;
+  joint_motion_period_us=period_us;
 }
 
-uint16_t joint_motion_get_period(void)
+inline void joint_motion_set_period(uint16_t period_us)
 {
-  return joint_motion_period;
+  joint_motion_period=(period_us<<16)/1000000;
+  joint_motion_period_us=period_us;
 }
 
-inline void joint_motion_load_target_angle(uint8_t servo_id, int16_t angle)
+inline uint16_t joint_motion_get_period(void)
 {
-  if(angle>manager_get_ccw_angle_limit(servo_id))
-    angle=manager_get_ccw_angle_limit(servo_id);
-  else if(angle<manager_get_cw_angle_limit(servo_id))
-    angle=manager_get_cw_angle_limit(servo_id);
-  joint_target_angles[servo_id]=angle<<9;
+  return joint_motion_period_us;
 }
 
-inline void joint_motion_load_target_speed(uint8_t servo_id, uint8_t speed)
+void joint_motion_set_group_servos(uint8_t group_id,uint32_t servos)
 {
-  joint_target_speeds[servo_id]=speed<<16;
+  joint_group_servos[group_id]=servos;
+  ram_data[DARWIN_JOINT_GRP0_SERVOS0+group_id*5]=servos&0x000000FF;
+  ram_data[DARWIN_JOINT_GRP0_SERVOS1+group_id*5]=(servos>>8)&0x000000FF;
+  ram_data[DARWIN_JOINT_GRP0_SERVOS2+group_id*5]=(servos>>16)&0x000000FF;
+  ram_data[DARWIN_JOINT_GRP0_SERVOS3+group_id*5]=(servos>>24)&0x000000FF;
 }
 
-inline void joint_motion_load_target_accel(uint8_t servo_id, uint8_t accel)
+uint32_t joint_motion_get_group_servos(uint8_t group_id)
 {
-  joint_target_accels[servo_id]=accel<<16;
+  return joint_group_servos[group_id];
 }
 
-void joint_motion_start(void)
+void joint_motion_start(uint8_t group_id)
 {
   uint8_t i;
+  int16_t angle;
+  uint32_t servo_index;
 
   /* initialize the internal variables */
-  for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
+  for(i=0,servo_index=0x00000001;i<MANAGER_MAX_NUM_SERVOS;i++,servo_index=servo_index<<1)
   {
-    // current values
-    joint_current_angles[i]=current_angles[i];
-    // the current angles, speeds and accelerations are in RAM
-    if(joint_target_angles[i]>=joint_current_angles[i])
-      joint_dir[i]=1;
-    else
-      joint_dir[i]=-1;
+    if(joint_group_servos[group_id]&servo_index)
+    {
+      if(ram_data[DARWIN_JOINT_SERVO0_SPEED+i]==0)
+      {
+        joint_target_angles[i]=manager_current_angles[i];
+        angle=manager_current_angles[i]>>9;
+        ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+i*2]=angle%256;
+        ram_data[DARWIN_JOINT_SERVO0_ANGLE_H+i*2]=angle/256;
+      }
+      else
+      {
+        // current values
+        joint_current_angles[i]=manager_current_angles[i];
+        // target angle
+        angle=ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+i*2]+(ram_data[DARWIN_JOINT_SERVO0_ANGLE_H+i*2]<<8);
+        if(angle>manager_get_ccw_angle_limit(i))
+          angle=manager_get_ccw_angle_limit(i);
+        else if(angle<manager_get_cw_angle_limit(i))
+          angle=manager_get_cw_angle_limit(i);
+        joint_target_angles[i]=angle<<9;
+        // target speed
+        joint_target_speeds[i]=ram_data[DARWIN_JOINT_SERVO0_SPEED+i]<<16;
+        // target speed
+        joint_target_accels[i]=ram_data[DARWIN_JOINT_SERVO0_ACCEL+i]<<16;
+        if(joint_target_accels[i]==0)
+        {
+          joint_target_accels[i]=1<<16;
+          ram_data[DARWIN_JOINT_SERVO0_ACCEL+i]=1;
+        }
+        // check the parameters
+        if(abs(joint_target_angles[i]-joint_current_angles[i])>=65)
+        {
+          if((joint_target_speeds[i]*joint_target_speeds[i])/joint_target_accels[i]>abs(joint_target_angles[i]-joint_current_angles[i]))
+          {
+            joint_target_accels[i]=(joint_target_speeds[i]*joint_target_speeds[i])/abs(joint_target_angles[i]-joint_current_angles[i]);
+            ram_data[DARWIN_JOINT_SERVO0_ACCEL+i]=joint_target_accels[i]>>16;
+          }
+        }
+        // the current angles, speeds and accelerations are in RAM
+        if(joint_target_angles[i]>=joint_current_angles[i])
+          joint_dir[i]=1;
+        else
+          joint_dir[i]=-1;
+      }
+    }
   }
-  joint_moving=0x01;
-  ram_set_bit(DARWIN_JOINTS_CONTROL,0);
-  ram_clear_bit(DARWIN_JOINTS_CONTROL,1);
-  ram_set_bit(DARWIN_JOINTS_STATUS,0);
+  joint_moving[group_id]=0x01;
+  ram_data[DARWIN_JOINT_GRP0_CNTRL+group_id*5]|=JOINT_STATUS;
+}
+
+void joint_motion_stop(uint8_t group_id)
+{
+  joint_stop[group_id]=0x01;
 }
 
-void joint_motion_stop(void)
+uint8_t are_joints_moving(uint8_t group_id)
 {
-  joint_stop=0x01;
-  ram_set_bit(DARWIN_JOINTS_CONTROL,1);
+  return joint_moving[group_id];
 }
 
-uint8_t are_joints_moving(void)
+// operation functions
+uint8_t joint_motion_in_range(unsigned short int address, unsigned short int length)
 {
-  return joint_moving;
+  if(ram_in_window(JOINT_BASE_ADDRESS,JOINT_MEM_LENGTH,address,length))
+    return 0x01;
+  else
+    return 0x00;
+}
+
+void joint_motion_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
+{
+
+}
+
+void joint_motion_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
+{
+  uint8_t j;
+  uint16_t i;
+  uint32_t servos;
+
+  // load motion information
+  for(i=DARWIN_JOINT_SERVO0_ANGLE_L;i<=DARWIN_JOINT_SERVO31_ACCEL;i++)
+    if(ram_in_range(i,address,length))
+      ram_data[i]=data[i-address];
+  // group servos
+  for(j=0;j<4;j++)
+  {
+    if(ram_in_window(DARWIN_JOINT_GRP0_SERVOS0+j*5,4,address,length))
+    {
+      servos=joint_motion_get_group_servos(j);
+      for(i=DARWIN_JOINT_GRP0_SERVOS0+j*5;i<=DARWIN_JOINT_GRP0_SERVOS3+j*5;i++)
+        if(ram_in_range(i,address,length))
+          servos|=data[i-address]<<(i*8);
+      joint_motion_set_group_servos(j,servos);
+    }
+    if(ram_in_range(DARWIN_JOINT_GRP0_CNTRL+j*5,address,length))
+    {
+      if(data[DARWIN_JOINT_GRP0_CNTRL+j*5-address]&JOINT_START)
+        joint_motion_start(j);
+      if(data[DARWIN_JOINT_GRP0_CNTRL+j*5-address]&JOINT_STOP)
+        joint_motion_stop(j);
+    }
+  }
 }
 
 // motion manager interface functions
 void joint_motion_process(void)
 {
-  uint8_t moving=0x00,i;
+  uint8_t moving,i,j;
+  uint32_t servo_index;
 
-  if(joint_moving==0x01)
+  for(j=0;j<4;j++)
   {
-    for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
+    moving=0x00;
+    if(joint_moving[j]==0x01)
     {
-      if(manager_get_module(i)==MM_JOINTS)
+      for(i=0,servo_index=0x00000001;i<MANAGER_MAX_NUM_SERVOS;i++,servo_index=servo_index<<1)
       {
-        if(joint_stop==0x01)
+        if(manager_get_module(i)==MM_JOINTS && joint_group_servos[j]&servo_index)
         {
-          joint_target_angles[i]=current_angles[i];
-          joint_target_speeds[i]=0;
-          joint_target_accels[i]=0;
-          joint_current_speeds[i]=0;
-        }
-        else
-        {
-          if(abs(joint_target_angles[i]-joint_current_angles[i])>=6553)// 0.5 degrees
-          {  
-            moving=0x01;
-            if(joint_current_speeds[i]!=joint_dir[i]*joint_target_speeds[i])
+          if(joint_stop[j]==0x01)
+          {
+            joint_target_angles[i]=manager_current_angles[i];
+            joint_target_speeds[i]=0;
+            joint_target_accels[i]=0;
+            joint_current_speeds[i]=0;
+          }
+          else
+          {
+            if(abs(joint_target_angles[i]-joint_current_angles[i])>=65)
             {
-              if(joint_current_speeds[i]>joint_dir[i]*joint_target_speeds[i])
-              {
-                joint_current_speeds[i]=joint_current_speeds[i]-((joint_target_accels[i]*joint_motion_period)>>16);
-                if(joint_current_speeds[i]<joint_dir[i]*joint_target_speeds[i])
-                  joint_current_speeds[i]=joint_dir[i]*joint_target_speeds[i];
-              }
-              else
+              moving=0x01;
+              if(joint_current_speeds[i]!=joint_dir[i]*joint_target_speeds[i])// it is necessary to change the current speed
               {
-                joint_current_speeds[i]=joint_current_speeds[i]+((joint_target_accels[i]*joint_motion_period)>>16);
                 if(joint_current_speeds[i]>joint_dir[i]*joint_target_speeds[i])
-                  joint_current_speeds[i]=joint_dir[i]*joint_target_speeds[i];
-              }
-              joint_current_angles[i]=joint_current_angles[i]+((joint_current_speeds[i]*joint_motion_period)>>16);
-              if(joint_dir[i]==1 && joint_current_angles[i]>joint_target_angles[i])
-                joint_current_angles[i]=joint_target_angles[i];
-              if(joint_dir[i]==-1 && joint_current_angles[i]<joint_target_angles[i])
-                joint_current_angles[i]=joint_target_angles[i];
-            }
-            else
-            {
-              if(abs(joint_target_angles[i]-joint_current_angles[i])>(joint_target_speeds[i]*joint_target_speeds[i]/(2*joint_target_accels[i])))
-              {
-                joint_current_angles[i]=joint_current_angles[i]+((joint_current_speeds[i]*joint_motion_period)>>16);
+                {
+                  joint_current_speeds[i]-=((joint_target_accels[i]*joint_motion_period)>>16);// reduce speed
+                  if(joint_current_speeds[i]<joint_dir[i]*joint_target_speeds[i])
+                    joint_current_speeds[i]=joint_dir[i]*joint_target_speeds[i];
+                }
+                else
+                {
+                  joint_current_speeds[i]+=((joint_target_accels[i]*joint_motion_period)>>16);// increase speed
+                  if(joint_current_speeds[i]>joint_dir[i]*joint_target_speeds[i])
+                    joint_current_speeds[i]=joint_dir[i]*joint_target_speeds[i];
+                }
+                joint_current_angles[i]+=((joint_current_speeds[i]*joint_motion_period)>>16);
                 if(joint_dir[i]==1 && joint_current_angles[i]>joint_target_angles[i])
                   joint_current_angles[i]=joint_target_angles[i];
                 if(joint_dir[i]==-1 && joint_current_angles[i]<joint_target_angles[i])
                   joint_current_angles[i]=joint_target_angles[i];
-              }
+              }  
               else
               {
-                joint_target_speeds[i]=joint_target_speeds[i]-((joint_target_accels[i]*joint_motion_period)>>16);
-                joint_current_speeds[i]=joint_dir[i]*joint_target_speeds[i];
-                joint_current_angles[i]=joint_current_angles[i]+((joint_current_speeds[i]*joint_motion_period)>>16);
-                if(joint_dir[i]==1 && joint_current_angles[i]>joint_target_angles[i])
-                  joint_current_angles[i]=joint_target_angles[i];
-                if(joint_dir[i]==-1 && joint_current_angles[i]<joint_target_angles[i])
-                  joint_current_angles[i]=joint_target_angles[i];
+                if(abs(joint_target_angles[i]-joint_current_angles[i])>(joint_target_speeds[i]*joint_target_speeds[i]/(2*joint_target_accels[i])))// constant speed phase
+                {
+                  joint_current_angles[i]+=((joint_current_speeds[i]*joint_motion_period)>>16);
+                  if(joint_dir[i]==1 && joint_current_angles[i]>joint_target_angles[i])
+                    joint_current_angles[i]=joint_target_angles[i];
+                  if(joint_dir[i]==-1 && joint_current_angles[i]<joint_target_angles[i])
+                    joint_current_angles[i]=joint_target_angles[i];
+                }
+                else// deceleration phase
+                {
+                  joint_target_speeds[i]=joint_target_speeds[i]-((joint_target_accels[i]*joint_motion_period)>>16);
+                  joint_current_speeds[i]=joint_dir[i]*joint_target_speeds[i];
+                  joint_current_angles[i]=joint_current_angles[i]+((joint_current_speeds[i]*joint_motion_period)>>16);
+                  if(joint_dir[i]==1 && joint_current_angles[i]>joint_target_angles[i])
+                    joint_current_angles[i]=joint_target_angles[i];
+                  if(joint_dir[i]==-1 && joint_current_angles[i]<joint_target_angles[i])
+                    joint_current_angles[i]=joint_target_angles[i];
+                }
               }
             }
+            manager_current_angles[i]=joint_current_angles[i];
+            manager_current_slopes[i]=5;
           }
-          current_angles[i]=joint_current_angles[i];
         }
       }
-    }
-    if(moving==0)
-    {
-      joint_moving=0x00;
-      ram_clear_bit(DARWIN_JOINTS_STATUS,0);
+      if(moving==0)
+      {
+        joint_moving[j]=0x00;
+        ram_data[DARWIN_JOINT_GRP0_CNTRL+j*5]&=(~JOINT_STATUS);
+      }
     }
   }
 }
diff --git a/src/motion_manager.c b/src/motion_manager.c
index a19992252053b04f9698dadf168f10ba28d9e38c..e24c877a8d2ba947beedbf7e2852e470f25d1c36 100755
--- a/src/motion_manager.c
+++ b/src/motion_manager.c
@@ -4,6 +4,9 @@
 #include "ram.h"
 #include "action.h"
 #include "walking.h"
+#include "joint_motion.h"
+#include "head_tracking.h"
+#include "imu.h"
 
 #define MANAGER_TIMER                   TIM5
 #define ENABLE_MANAGER_TIMER_CLK        __HAL_RCC_TIM5_CLK_ENABLE()
@@ -108,36 +111,27 @@ void manager_get_target_position(void)
 
 void manager_balance(void)
 {
-  //adc_ch_t fb_ch,lr_ch;
   int16_t knee_gain,ankle_roll_gain,ankle_pitch_gain,hip_roll_gain;
-  //uint16_t fb_offset,lr_offset;
-  int16_t fb_value,lr_value;
+  int32_t gyro_x,gyro_y,gyro_z;
 
   if(manager_balance_enabled==0x01)// balance is enabled
   {
-    //fb_ch=gyro_get_fb_adc_channel();
-    //lr_ch=gyro_get_lr_adc_channel();
     // get the balance gains
     knee_gain=ram_data[DARWIN_MM_BAL_KNEE_GAIN_L]+(ram_data[DARWIN_MM_BAL_KNEE_GAIN_H]<<8);
     ankle_roll_gain=ram_data[DARWIN_MM_BAL_ANKLE_ROLL_GAIN_L]+(ram_data[DARWIN_MM_BAL_ANKLE_ROLL_GAIN_H]<<8);
     ankle_pitch_gain=ram_data[DARWIN_MM_BAL_ANKLE_PITCH_GAIN_L]+(ram_data[DARWIN_MM_BAL_ANKLE_PITCH_GAIN_H]<<8);
     hip_roll_gain=ram_data[DARWIN_MM_BAL_HIP_ROLL_GAIN_L]+(ram_data[DARWIN_MM_BAL_HIP_ROLL_GAIN_H]<<8);
-    // get the offsets of the gyroscope calibration
-    //gyro_get_offsets(&fb_offset,&lr_offset);
     // get the values of the gyroscope
-    //fb_value=adc_get_channel_raw(fb_ch)-fb_offset;
-    //lr_value=adc_get_channel_raw(lr_ch)-lr_offset;
-    fb_value=0;
-    lr_value=0;
+    imu_get_gyro_data(&gyro_x,&gyro_y,&gyro_z);
     // compensate the servo angle values
-    manager_balance_offset[R_KNEE]=(((fb_value*manager_servos[R_KNEE].max_angle)/manager_servos[R_KNEE].encoder_resolution)*knee_gain)>>16;
-    manager_balance_offset[R_ANKLE_PITCH]=(((fb_value*manager_servos[R_ANKLE_PITCH].max_angle)/manager_servos[R_ANKLE_PITCH].encoder_resolution)*ankle_pitch_gain)>>16;    
-    manager_balance_offset[L_KNEE]=-(((fb_value*manager_servos[L_KNEE].max_angle)/manager_servos[L_KNEE].encoder_resolution)*knee_gain)>>16;    
-    manager_balance_offset[L_ANKLE_PITCH]=-(((fb_value*manager_servos[L_ANKLE_PITCH].max_angle)/manager_servos[L_ANKLE_PITCH].encoder_resolution)*ankle_pitch_gain)>>16;
-    manager_balance_offset[R_HIP_ROLL]=(((lr_value*manager_servos[R_HIP_ROLL].max_angle)/manager_servos[R_HIP_ROLL].encoder_resolution)*hip_roll_gain)>>16;
-    manager_balance_offset[L_HIP_ROLL]=(((lr_value*manager_servos[L_HIP_ROLL].max_angle)/manager_servos[L_HIP_ROLL].encoder_resolution)*hip_roll_gain)>>16;
-    manager_balance_offset[R_ANKLE_ROLL]=-(((lr_value*manager_servos[R_ANKLE_ROLL].max_angle)/manager_servos[R_ANKLE_ROLL].encoder_resolution)*ankle_roll_gain)>>16;
-    manager_balance_offset[L_ANKLE_ROLL]=-(((lr_value*manager_servos[L_ANKLE_ROLL].max_angle)/manager_servos[L_ANKLE_ROLL].encoder_resolution)*ankle_roll_gain)>>16;
+    manager_balance_offset[R_KNEE]=(((gyro_y*manager_servos[R_KNEE].max_angle)/manager_servos[R_KNEE].encoder_resolution)*knee_gain)>>16;
+    manager_balance_offset[R_ANKLE_PITCH]=(((gyro_y*manager_servos[R_ANKLE_PITCH].max_angle)/manager_servos[R_ANKLE_PITCH].encoder_resolution)*ankle_pitch_gain)>>16;    
+    manager_balance_offset[L_KNEE]=-(((gyro_y*manager_servos[L_KNEE].max_angle)/manager_servos[L_KNEE].encoder_resolution)*knee_gain)>>16;    
+    manager_balance_offset[L_ANKLE_PITCH]=-(((gyro_y*manager_servos[L_ANKLE_PITCH].max_angle)/manager_servos[L_ANKLE_PITCH].encoder_resolution)*ankle_pitch_gain)>>16;
+    manager_balance_offset[R_HIP_ROLL]=(((gyro_x*manager_servos[R_HIP_ROLL].max_angle)/manager_servos[R_HIP_ROLL].encoder_resolution)*hip_roll_gain)>>16;
+    manager_balance_offset[L_HIP_ROLL]=(((gyro_x*manager_servos[L_HIP_ROLL].max_angle)/manager_servos[L_HIP_ROLL].encoder_resolution)*hip_roll_gain)>>16;
+    manager_balance_offset[R_ANKLE_ROLL]=-(((gyro_x*manager_servos[R_ANKLE_ROLL].max_angle)/manager_servos[R_ANKLE_ROLL].encoder_resolution)*ankle_roll_gain)>>16;
+    manager_balance_offset[L_ANKLE_ROLL]=-(((gyro_x*manager_servos[L_ANKLE_ROLL].max_angle)/manager_servos[L_ANKLE_ROLL].encoder_resolution)*ankle_roll_gain)>>16;
   }
 }
 
@@ -156,7 +150,9 @@ void MANAGER_TIMER_IRQHandler(void)
       // call the action process
       action_process();
       // call the joint motion process
-      // joint_motion_process();
+      joint_motion_process();
+      // call the head_tracking process
+      head_tracking_process();
       // call the walking process
       walking_process();
       // balance the robot
@@ -333,6 +329,8 @@ void manager_init(uint16_t period_us)
   /* initialize action module */
   action_init(period_us);
   walking_init(period_us);
+  joint_motion_init(period_us);
+  head_tracking_init(period_us);
 }
 
 uint16_t manager_get_period(void)
@@ -353,6 +351,8 @@ void manager_set_period(uint16_t period_us)
   ram_data[DARWIN_MM_PERIOD_H]=(period_us&0xFF00)>>8;
   action_set_period(period_us);
   walking_set_period(period_us);
+  joint_motion_set_period(period_us);
+  head_tracking_init(period_us);
 }
 
 inline void manager_enable(void)
diff --git a/src/ram.c b/src/ram.c
index fdec190780ca59ae34ba6e2c5123cef1ca127a86..cf821c30d488f1062058d3420b40dadda31b8efc 100755
--- a/src/ram.c
+++ b/src/ram.c
@@ -87,6 +87,38 @@ void ram_init(void)
     ram_data[WALK_MAX_VEL]=(uint8_t)(eeprom_data&0x00FF);
   if(EE_ReadVariable(WALK_MAX_ROT_VEL,&eeprom_data)==0)
     ram_data[WALK_MAX_ROT_VEL]=(uint8_t)(eeprom_data&0x00FF);
+  if(EE_ReadVariable(HEAD_PAN_P,&eeprom_data)==0)
+    ram_data[HEAD_PAN_P]=(uint8_t)(eeprom_data&0x00FF);
+  if(EE_ReadVariable(HEAD_PAN_P+1,&eeprom_data)==0)
+    ram_data[HEAD_PAN_P+1]=(uint8_t)(eeprom_data&0x00FF);
+  if(EE_ReadVariable(HEAD_PAN_I,&eeprom_data)==0)
+    ram_data[HEAD_PAN_I]=(uint8_t)(eeprom_data&0x00FF);
+  if(EE_ReadVariable(HEAD_PAN_I+1,&eeprom_data)==0)
+    ram_data[HEAD_PAN_I+1]=(uint8_t)(eeprom_data&0x00FF);
+  if(EE_ReadVariable(HEAD_PAN_D,&eeprom_data)==0)
+    ram_data[HEAD_PAN_D]=(uint8_t)(eeprom_data&0x00FF);
+  if(EE_ReadVariable(HEAD_PAN_D+1,&eeprom_data)==0)
+    ram_data[HEAD_PAN_D+1]=(uint8_t)(eeprom_data&0x00FF);
+  if(EE_ReadVariable(HEAD_PAN_I_CLAMP,&eeprom_data)==0)
+    ram_data[HEAD_PAN_I_CLAMP]=(uint8_t)(eeprom_data&0x00FF);
+  if(EE_ReadVariable(HEAD_PAN_I_CLAMP+1,&eeprom_data)==0)
+    ram_data[HEAD_PAN_I_CLAMP+1]=(uint8_t)(eeprom_data&0x00FF);
+  if(EE_ReadVariable(HEAD_TILT_P,&eeprom_data)==0)
+    ram_data[HEAD_TILT_P]=(uint8_t)(eeprom_data&0x00FF);
+  if(EE_ReadVariable(HEAD_TILT_P+1,&eeprom_data)==0)
+    ram_data[HEAD_TILT_P+1]=(uint8_t)(eeprom_data&0x00FF);
+  if(EE_ReadVariable(HEAD_TILT_I,&eeprom_data)==0)
+    ram_data[HEAD_TILT_I]=(uint8_t)(eeprom_data&0x00FF);
+  if(EE_ReadVariable(HEAD_TILT_I+1,&eeprom_data)==0)
+    ram_data[HEAD_TILT_I+1]=(uint8_t)(eeprom_data&0x00FF);
+  if(EE_ReadVariable(HEAD_TILT_D,&eeprom_data)==0)
+    ram_data[HEAD_TILT_D]=(uint8_t)(eeprom_data&0x00FF);
+  if(EE_ReadVariable(HEAD_TILT_D+1,&eeprom_data)==0)
+    ram_data[HEAD_TILT_D+1]=(uint8_t)(eeprom_data&0x00FF);
+  if(EE_ReadVariable(HEAD_TILT_I_CLAMP,&eeprom_data)==0)
+    ram_data[HEAD_TILT_I_CLAMP]=(uint8_t)(eeprom_data&0x00FF);
+  if(EE_ReadVariable(HEAD_TILT_I_CLAMP+1,&eeprom_data)==0)
+    ram_data[HEAD_TILT_I_CLAMP+1]=(uint8_t)(eeprom_data&0x00FF);
 }
 
 inline void ram_read_byte(uint16_t address,uint8_t *data)