Added the old implementation of the motion modules.

dynamixel_manager/include/modules/old/darwin_kinematics.h

+#ifndef _DARWIN_KINEMATICS_H
+#define _DARWIN_KINEMATICS_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "stm32f1xx.h"
+
+#define DARWIN_PELVIS_LENGTH               29.0 //mm
+#define DARWIN_LEG_SIDE_OFFSET             37.0 //mm
+#define DARWIN_THIGH_LENGTH                93.0 //mm
+#define DARWIN_CALF_LENGTH                 93.0 //mm
+#define DARWIN_ANKLE_LENGTH                33.5 //mm
+#define DARWIN_LEG_LENGTH                  (DARWIN_THIGH_LENGTH + DARWIN_CALF_LENGTH + DARWIN_ANKLE_LENGTH) //mm (THIGH_LENGTH + CALF_LENGTH + ANKLE_LENGTH)
+
+// public functions
+uint8_t darwin_leg_ik(float *out, float x, float y, float z, float a, float b, float c);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

dynamixel_manager/include/modules/old/darwin_math.h

+#ifndef _DARWIN_MATH_H
+#define _DARWIN_MATH_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "stm32f1xx.h"
+
+#define   PI    3.14159
+
+typedef struct{
+  float x;
+  float y;
+  float z;
+}TPoint3D;
+
+// point public functions
+void point3d_init(TPoint3D *point);
+void point3d_load(TPoint3D *point, float x, float y, float z);
+void point3d_copy(TPoint3D *point_dst, TPoint3D *point_src);
+
+typedef struct {
+  float x;
+  float y;
+  float z;
+}TVector3D;
+
+// vector public functions
+void vector3d_init(TVector3D *vector);
+void vector3d_load(TVector3D *vector, float x, float y, float z);
+void vector3d_copy(TVector3D *vector_dst, TVector3D *vector_src);
+float vector3d_length(TVector3D *vector);
+
+enum{
+  m00=0,
+  m01,
+  m02,
+  m03,
+  m10,
+  m11,
+  m12,
+  m13,
+  m20,
+  m21,
+  m22,
+  m23,
+  m30,
+  m31,
+  m32,
+  m33,
+};
+
+typedef struct{
+  float coef[16];
+}TMatrix3D;
+
+// matrix public functions
+void matrix3d_init(TMatrix3D *matrix);
+void matrix3d_copy(TMatrix3D *matrix_dst, TMatrix3D *matrix_src);
+void matrix3d_identity(TMatrix3D *matrix);
+uint8_t matrix3d_inverse(TMatrix3D *org, TMatrix3D *inv);
+void matrix3d_set_transform(TMatrix3D *matrix, TPoint3D *point, TVector3D *vector);
+void matrix3d_mult(TMatrix3D *dst,TMatrix3D *a,TMatrix3D *b);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

dynamixel_manager/include/modules/old/grippers.h

+#ifndef _GRIPPERS_H
+#define _GRIPPERS_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "stm32f1xx.h"
+#include "motion_manager.h"
+
+typedef enum {GRIPPER_LEFT,GRIPPER_RIGHT} grippers_t;
+
+// public functions
+void grippers_init(uint16_t period_us);
+void grippers_set_period(uint16_t period_us);
+uint16_t grippers_get_period(void);
+void grippers_open(grippers_t gripper);
+void grippers_close(grippers_t gripper);
+void grippers_stop(grippers_t gripper);
+uint8_t gripper_is_open(grippers_t gripper);
+uint8_t gripper_is_close(grippers_t gripper);
+uint8_t gripper_is_moving(grippers_t gripper);
+
+// operation functions
+uint8_t grippers_in_range(unsigned short int address, unsigned short int length);
+void grippers_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
+void grippers_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
+void grippers_process(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

dynamixel_manager/include/modules/old/head_tracking.h

+#ifndef _HEAD_TRACKING_H
+#define _HEAD_TRACKINH_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "stm32f1xx.h"
+#include "motion_manager.h"
+
+// public functions
+void head_tracking_init(uint16_t period_us);
+void head_tracking_set_period(uint16_t period_us);
+uint16_t head_tracking_get_period(void);
+void head_tracking_start(void);
+void head_tracking_stop(void);
+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

dynamixel_manager/include/modules/old/joint_motion.h

+#ifndef _JOINT_MOTION_H
+#define _JOINT_MOTION_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_us);
+void joint_motion_set_period(uint16_t period_us);
+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

dynamixel_manager/include/modules/old/stairs.h

+#ifndef _STAIRS_H
+#define _STAIRS_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "stm32f1xx.h"
+#include "motion_manager.h"
+
+// public functions
+void stairs_init(uint16_t period_us);
+void stairs_set_period(uint16_t period_us);
+uint16_t stairs_get_period(void);
+void stairs_start(uint8_t up);
+void stairs_stop(void);
+uint8_t is_climbing_stairs(void);
+uint8_t stairs_get_phase(void);
+
+// operation functions
+uint8_t stairs_in_range(unsigned short int address, unsigned short int length);
+void stairs_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
+void stairs_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
+// motion manager interface functions
+void stairs_process(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

dynamixel_manager/include/modules/old/walking.h

+#ifndef _WALKING_H
+#define _WALKING_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "stm32f1xx.h"
+#include "motion_manager.h"
+
+// public functions
+void walking_init(uint16_t period_us);
+void walking_set_period(uint16_t period_us);
+uint16_t walking_get_period(void);
+void walking_start(void);
+void walking_stop(void);
+uint8_t is_walking(void);
+
+// operation functions
+uint8_t walking_in_range(unsigned short int address, unsigned short int length);
+void walking_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
+void walking_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
+// motion manager interface functions
+void walking_process(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

dynamixel_manager/src/modules/old/darwin_kinematics.c

+#include "darwin_kinematics.h"
+#include "darwin_math.h"
+#include <math.h>
+
+uint8_t darwin_leg_ik(float *out, float x, float y, float z, float a, float b, float c)
+{
+  TMatrix3D Tad, Tda, Tcd, Tdc, Tac;
+  TVector3D vec,orientation;
+  TPoint3D position;
+  float _Rac, _Acos, _Atan, _k, _l, _m, _n, _s, _c, _theta;
+
+  vector3d_load(&orientation,a * 180.0 / PI, b * 180.0 / PI, c * 180.0 / PI);
+  point3d_load(&position,x, y, z - DARWIN_LEG_LENGTH);
+  matrix3d_set_transform(&Tad,&position,&orientation);
+
+  vec.x = x + Tad.coef[2] * DARWIN_ANKLE_LENGTH;
+  vec.y = y + Tad.coef[6] * DARWIN_ANKLE_LENGTH;
+  vec.z = (z - DARWIN_LEG_LENGTH) + Tad.coef[10] * DARWIN_ANKLE_LENGTH;
+
+  // Get Knee
+  _Rac = vector3d_length(&vec);
+  _Acos = acos((_Rac * _Rac - DARWIN_THIGH_LENGTH * DARWIN_THIGH_LENGTH - DARWIN_CALF_LENGTH * DARWIN_CALF_LENGTH) / (2 * DARWIN_THIGH_LENGTH * DARWIN_CALF_LENGTH));
+  if(isnan(_Acos) == 1)
+    return 0x00;;
+  *(out + 3) = _Acos;
+
+  // Get Ankle Roll
+  Tda = Tad;
+  if(matrix3d_inverse(&Tda,&Tda) == 0x00)
+    return 0x00;
+  _k = sqrt(Tda.coef[7] * Tda.coef[7] + Tda.coef[11] * Tda.coef[11]);
+  _l = sqrt(Tda.coef[7] * Tda.coef[7] + (Tda.coef[11] - DARWIN_ANKLE_LENGTH) * (Tda.coef[11] - DARWIN_ANKLE_LENGTH));
+  _m = (_k * _k - _l * _l - DARWIN_ANKLE_LENGTH * DARWIN_ANKLE_LENGTH) / (2 * _l * DARWIN_ANKLE_LENGTH);
+  if(_m > 1.0)
+    _m = 1.0;
+  else if(_m < -1.0)
+    _m = -1.0;
+  _Acos = acos(_m);
+  if(isnan(_Acos) == 1)
+    return 0x00;
+  if(Tda.coef[7] < 0.0)
+    *(out + 5) = -_Acos;
+  else
+    *(out + 5) = _Acos;
+  // Get Hip Yaw
+  vector3d_load(&orientation,*(out + 5) * 180.0 / PI, 0, 0);
+  point3d_load(&position,0, 0, -DARWIN_ANKLE_LENGTH);
+  matrix3d_set_transform(&Tcd,&position,&orientation);
+  Tdc = Tcd;
+  if(matrix3d_inverse(&Tdc,&Tdc) == 0x00)
+    return 0x00;
+  matrix3d_mult(&Tac,&Tad,&Tdc);
+  _Atan = atan2(-Tac.coef[1] , Tac.coef[5]);
+  if(isinf(_Atan) == 1)
+    return 0x00;
+  *(out) = _Atan;
+
+  // Get Hip Roll
+  _Atan = atan2(Tac.coef[9], -Tac.coef[1] * sin(*(out)) + Tac.coef[5] * cos(*(out)));
+  if(isinf(_Atan) == 1)
+    return 0x00;
+  *(out + 1) = _Atan;
+
+  // Get Hip Pitch and Ankle Pitch
+  _Atan = atan2(Tac.coef[2] * cos(*(out)) + Tac.coef[6] * sin(*(out)), Tac.coef[0] * cos(*(out)) + Tac.coef[4] * sin(*(out)));
+  if(isinf(_Atan) == 1)
+    return 0x00;
+  _theta = _Atan;
+  _k = sin(*(out + 3)) * DARWIN_CALF_LENGTH;
+  _l = -DARWIN_THIGH_LENGTH - cos(*(out + 3)) * DARWIN_CALF_LENGTH;
+  _m = cos(*(out)) * vec.x + sin(*(out)) * vec.y;
+  _n = cos(*(out + 1)) * vec.z + sin(*(out)) * sin(*(out + 1)) * vec.x - cos(*(out)) * sin(*(out + 1)) * vec.y;
+  _s = (_k * _n + _l * _m) / (_k * _k + _l * _l);
+  _c = (_n - _k * _s) / _l;
+  _Atan = atan2(_s, _c);
+  if(isinf(_Atan) == 1)
+    return 0x00;
+  *(out + 2) = _Atan;
+  *(out + 4) = _theta - *(out + 3) - *(out + 2);
+
+  return 0x01;
+}

dynamixel_manager/src/modules/old/darwin_math.c

+#include "darwin_math.h"
+#include <math.h>
+
+// point functions
+void point3d_init(TPoint3D *point)
+{
+  point->x=0.0;
+  point->y=0.0;
+  point->z=0.0;
+}
+
+void point3d_load(TPoint3D *point, float x, float y,float z)
+{
+  point->x=x;
+  point->y=y;
+  point->z=z;  
+}
+
+void point3d_copy(TPoint3D *point_dst, TPoint3D *point_src)
+{
+  point_dst->x=point_src->x;
+  point_dst->y=point_src->y;
+  point_dst->z=point_src->z;
+}
+
+// vector functions
+void vector3d_init(TVector3D *vector)
+{
+  vector->x=0.0;
+  vector->y=0.0;
+  vector->z=0.0;
+}
+
+void vector3d_load(TVector3D *vector, float x, float y, float z)
+{
+  vector->x=x;
+  vector->y=y;
+  vector->z=z;
+}
+
+void vector3d_copy(TVector3D *vector_dst, TVector3D *vector_src)
+{
+  vector_dst->x=vector_src->x;
+  vector_dst->y=vector_src->y;
+  vector_dst->z=vector_src->z;
+}
+
+float vector3d_length(TVector3D *vector)
+{
+  return sqrt(vector->x*vector->x + vector->y*vector->y + vector->z*vector->z); 
+}
+
+// matrix functions
+void matrix3d_init(TMatrix3D *matrix)
+{
+   matrix3d_identity(matrix);
+}
+
+void matrix3d_copy(TMatrix3D *matrix_dst, TMatrix3D *matrix_src)
+{
+  matrix_dst->coef[m00]=matrix_src->coef[m00];
+  matrix_dst->coef[m01]=matrix_src->coef[m01];
+  matrix_dst->coef[m02]=matrix_src->coef[m02];
+  matrix_dst->coef[m03]=matrix_src->coef[m03];
+  matrix_dst->coef[m10]=matrix_src->coef[m10];
+  matrix_dst->coef[m11]=matrix_src->coef[m11];
+  matrix_dst->coef[m12]=matrix_src->coef[m12];
+  matrix_dst->coef[m13]=matrix_src->coef[m13];
+  matrix_dst->coef[m20]=matrix_src->coef[m20];
+  matrix_dst->coef[m21]=matrix_src->coef[m21];
+  matrix_dst->coef[m22]=matrix_src->coef[m22];
+  matrix_dst->coef[m23]=matrix_src->coef[m23];
+  matrix_dst->coef[m30]=matrix_src->coef[m30];
+  matrix_dst->coef[m31]=matrix_src->coef[m31];
+  matrix_dst->coef[m32]=matrix_src->coef[m32];
+  matrix_dst->coef[m33]=matrix_src->coef[m33];
+}
+
+void matrix3d_zero(TMatrix3D *matrix)
+{
+  matrix->coef[m00]=0.0;
+  matrix->coef[m01]=0.0;
+  matrix->coef[m02]=0.0;
+  matrix->coef[m03]=0.0;
+  matrix->coef[m10]=0.0;
+  matrix->coef[m11]=0.0;
+  matrix->coef[m12]=0.0;
+  matrix->coef[m13]=0.0;
+  matrix->coef[m20]=0.0;
+  matrix->coef[m21]=0.0;
+  matrix->coef[m22]=0.0;
+  matrix->coef[m23]=0.0;
+  matrix->coef[m30]=0.0;
+  matrix->coef[m31]=0.0;
+  matrix->coef[m32]=0.0;
+  matrix->coef[m33]=0.0;
+}
+
+void matrix3d_identity(TMatrix3D *matrix)
+{
+  matrix->coef[m00]=1.0;
+  matrix->coef[m01]=0.0;
+  matrix->coef[m02]=0.0;
+  matrix->coef[m03]=0.0;
+  matrix->coef[m10]=0.0;
+  matrix->coef[m11]=1.0;
+  matrix->coef[m12]=0.0;
+  matrix->coef[m13]=0.0;
+  matrix->coef[m20]=0.0;
+  matrix->coef[m21]=0.0;
+  matrix->coef[m22]=1.0;
+  matrix->coef[m23]=0.0;
+  matrix->coef[m30]=0.0;
+  matrix->coef[m31]=0.0;
+  matrix->coef[m32]=0.0;
+  matrix->coef[m33]=1.0;
+}
+
+uint8_t matrix3d_inverse(TMatrix3D *org, TMatrix3D *inv)
+{
+  TMatrix3D src, tmp;
+  float det;
+  uint8_t i;
+
+  /* transpose matrix */
+  for (i = 0; i < 4; i++)
+  {
+    src.coef[i] = org->coef[i*4];
+    src.coef[i + 4] = org->coef[i*4 + 1];
+    src.coef[i + 8] = org->coef[i*4 + 2];
+    src.coef[i + 12] = org->coef[i*4 + 3];
+  }
+
+  /* calculate pairs for first 8 elements (cofactors) */
+  tmp.coef[0] = src.coef[10] * src.coef[15];
+  tmp.coef[1] = src.coef[11] * src.coef[14];
+  tmp.coef[2] = src.coef[9] * src.coef[15];
+  tmp.coef[3] = src.coef[11] * src.coef[13];
+  tmp.coef[4] = src.coef[9] * src.coef[14];
+  tmp.coef[5] = src.coef[10] * src.coef[13];
+  tmp.coef[6] = src.coef[8] * src.coef[15];
+  tmp.coef[7] = src.coef[11] * src.coef[12];
+  tmp.coef[8] = src.coef[8] * src.coef[14];
+  tmp.coef[9] = src.coef[10] * src.coef[12];
+  tmp.coef[10] = src.coef[8] * src.coef[13];
+  tmp.coef[11] = src.coef[9] * src.coef[12];
+  /* calculate first 8 elements (cofactors) */
+  inv->coef[0] = (tmp.coef[0]*src.coef[5] + tmp.coef[3]*src.coef[6] + tmp.coef[4]*src.coef[7]) - (tmp.coef[1]*src.coef[5] + tmp.coef[2]*src.coef[6] + tmp.coef[5]*src.coef[7]);
+  inv->coef[1] = (tmp.coef[1]*src.coef[4] + tmp.coef[6]*src.coef[6] + tmp.coef[9]*src.coef[7]) - (tmp.coef[0]*src.coef[4] + tmp.coef[7]*src.coef[6] + tmp.coef[8]*src.coef[7]);
+  inv->coef[2] = (tmp.coef[2]*src.coef[4] + tmp.coef[7]*src.coef[5] + tmp.coef[10]*src.coef[7]) - (tmp.coef[3]*src.coef[4] + tmp.coef[6]*src.coef[5] + tmp.coef[11]*src.coef[7]);
+  inv->coef[3] = (tmp.coef[5]*src.coef[4] + tmp.coef[8]*src.coef[5] + tmp.coef[11]*src.coef[6]) - (tmp.coef[4]*src.coef[4] + tmp.coef[9]*src.coef[5] + tmp.coef[10]*src.coef[6]);
+  inv->coef[4] = (tmp.coef[1]*src.coef[1] + tmp.coef[2]*src.coef[2] + tmp.coef[5]*src.coef[3]) - (tmp.coef[0]*src.coef[1] + tmp.coef[3]*src.coef[2] + tmp.coef[4]*src.coef[3]);
+  inv->coef[5] = (tmp.coef[0]*src.coef[0] + tmp.coef[7]*src.coef[2] + tmp.coef[8]*src.coef[3]) - (tmp.coef[1]*src.coef[0] + tmp.coef[6]*src.coef[2] + tmp.coef[9]*src.coef[3]);
+  inv->coef[6] = (tmp.coef[3]*src.coef[0] + tmp.coef[6]*src.coef[1] + tmp.coef[11]*src.coef[3]) - (tmp.coef[2]*src.coef[0] + tmp.coef[7]*src.coef[1] + tmp.coef[10]*src.coef[3]);
+  inv->coef[7] = (tmp.coef[4]*src.coef[0] + tmp.coef[9]*src.coef[1] + tmp.coef[10]*src.coef[2]) - (tmp.coef[5]*src.coef[0] + tmp.coef[8]*src.coef[1] + tmp.coef[11]*src.coef[2]);
+  /* calculate pairs for second 8 elements (cofactors) */
+  tmp.coef[0] = src.coef[2]*src.coef[7];
+  tmp.coef[1] = src.coef[3]*src.coef[6];
+  tmp.coef[2] = src.coef[1]*src.coef[7];
+  tmp.coef[3] = src.coef[3]*src.coef[5];
+  tmp.coef[4] = src.coef[1]*src.coef[6];
+  tmp.coef[5] = src.coef[2]*src.coef[5];
+  //Streaming SIMD Extensions - Inverse of 4x4 Matrix 8
+  tmp.coef[6] = src.coef[0]*src.coef[7];
+  tmp.coef[7] = src.coef[3]*src.coef[4];
+  tmp.coef[8] = src.coef[0]*src.coef[6];
+  tmp.coef[9] = src.coef[2]*src.coef[4];
+  tmp.coef[10] = src.coef[0]*src.coef[5];
+  tmp.coef[11] = src.coef[1]*src.coef[4];
+  /* calculate second 8 elements (cofactors) */
+  inv->coef[8] = (tmp.coef[0]*src.coef[13] + tmp.coef[3]*src.coef[14] + tmp.coef[4]*src.coef[15]) - (tmp.coef[1]*src.coef[13] + tmp.coef[2]*src.coef[14] + tmp.coef[5]*src.coef[15]);
+  inv->coef[9] = (tmp.coef[1]*src.coef[12] + tmp.coef[6]*src.coef[14] + tmp.coef[9]*src.coef[15]) - (tmp.coef[0]*src.coef[12] + tmp.coef[7]*src.coef[14] + tmp.coef[8]*src.coef[15]);
+  inv->coef[10] = (tmp.coef[2]*src.coef[12] + tmp.coef[7]*src.coef[13] + tmp.coef[10]*src.coef[15]) - (tmp.coef[3]*src.coef[12] + tmp.coef[6]*src.coef[13] + tmp.coef[11]*src.coef[15]);
+  inv->coef[11] = (tmp.coef[5]*src.coef[12] + tmp.coef[8]*src.coef[13] + tmp.coef[11]*src.coef[14]) - (tmp.coef[4]*src.coef[12] + tmp.coef[9]*src.coef[13] + tmp.coef[10]*src.coef[14]);
+  inv->coef[12] = (tmp.coef[2]*src.coef[10] + tmp.coef[5]*src.coef[11] + tmp.coef[1]*src.coef[9]) - (tmp.coef[4]*src.coef[11] + tmp.coef[0]*src.coef[9] + tmp.coef[3]*src.coef[10]);
+  inv->coef[13] = (tmp.coef[8]*src.coef[11] + tmp.coef[0]*src.coef[8] + tmp.coef[7]*src.coef[10]) - (tmp.coef[6]*src.coef[10] + tmp.coef[9]*src.coef[11] + tmp.coef[1]*src.coef[8]);
+  inv->coef[14] = (tmp.coef[6]*src.coef[9] + tmp.coef[11]*src.coef[11] + tmp.coef[3]*src.coef[8]) - (tmp.coef[10]*src.coef[11] + tmp.coef[2]*src.coef[8] + tmp.coef[7]*src.coef[9]);
+  inv->coef[15] = (tmp.coef[10]*src.coef[10] + tmp.coef[4]*src.coef[8] + tmp.coef[9]*src.coef[9]) - (tmp.coef[8]*src.coef[9] + tmp.coef[11]*src.coef[10] + tmp.coef[5]*src.coef[8]);
+  /* calculate determinant */
+  det = src.coef[0]*inv->coef[0] + src.coef[1]*inv->coef[1] + src.coef[2]*inv->coef[2] + src.coef[3]*inv->coef[3];
+  /* calculate matrix inverse */
+  if (det == 0)
+  {
+    det = 0;
+    return 0x00;
+  }
+  else
+  {
+    det = 1 / det;
+  }
+
+  for (i = 0; i < 16; i++)
+    inv->coef[i]*=det;
+
+  return 0x01;
+}
+
+void matrix3d_set_transform(TMatrix3D *matrix, TPoint3D *point, TVector3D *vector)
+{
+  float Cx = cos(vector->x * 3.141592 / 180.0);
+  float Cy = cos(vector->y * 3.141592 / 180.0);
+  float Cz = cos(vector->z * 3.141592 / 180.0);
+  float Sx = sin(vector->x * 3.141592 / 180.0);
+  float Sy = sin(vector->y * 3.141592 / 180.0);
+  float Sz = sin(vector->z * 3.141592 / 180.0);
+
+  matrix3d_identity(matrix);
+  matrix->coef[0] = Cz * Cy;
+  matrix->coef[1] = Cz * Sy * Sx - Sz * Cx;
+  matrix->coef[2] = Cz * Sy * Cx + Sz * Sx;
+  matrix->coef[3] = point->x;
+  matrix->coef[4] = Sz * Cy;
+  matrix->coef[5] = Sz * Sy * Sx + Cz * Cx;
+  matrix->coef[6] = Sz * Sy * Cx - Cz * Sx;
+  matrix->coef[7] = point->y;
+  matrix->coef[8] = -Sy;
+  matrix->coef[9] = Cy * Sx;
+  matrix->coef[10] = Cy * Cx;
+  matrix->coef[11] = point->z;
+}
+
+void matrix3d_mult(TMatrix3D *dst,TMatrix3D *a,TMatrix3D *b)
+{
+  uint8_t i,j,c;
+
+  matrix3d_zero(dst);
+  for(j = 0; j < 4; j++)
+  {
+    for(i = 0; i < 4; i++)
+    {
+      for(c = 0; c < 4; c++)
+      {
+         dst->coef[j*4+i] += a->coef[j*4+c] * b->coef[c*4+i];
+      }
+    }
+  }
+}

dynamixel_manager/src/modules/old/grippers.c

+#include "darwin_dyn_master_v2.h"
+#include "dyn_servos.h"
+#include "grippers.h"
+#include "ram.h"
+#include "joint_motion.h"
+
+// private variables
+int64_t grippers_period;
+int16_t grippers_period_us;
+uint8_t gripper_left_target;
+uint8_t gripper_right_target;
+
+// public functions
+void grippers_init(uint16_t period_us)
+{
+  uint16_t force;
+  uint32_t servo_mask=0x00000000;
+
+  /* initialize private variables */
+  gripper_left_target=0x00;//close;
+  gripper_right_target=0x00;//close;
+  grippers_period=(period_us<<16)/1000000;
+  grippers_period_us=period_us;
+  /* configure the maximum force of the servos */
+  servo_mask=0x00000000;
+  if(ram_data[DARWIN_GRIPPER_LEFT_TOP_ID]!=0xFF && ram_data[DARWIN_GRIPPER_LEFT_BOT_ID]!=0xFF)
+  {
+    force=ram_data[DARWIN_GRIPPER_LEFT_MAX_FORCE_L]+(ram_data[DARWIN_GRIPPER_LEFT_MAX_FORCE_H]<<8);
+    dyn_master_write_word(&darwin_dyn_master_v2,ram_data[DARWIN_GRIPPER_LEFT_TOP_ID],XL_TORQUE_LIMIT_L,force);
+    dyn_master_write_word(&darwin_dyn_master_v2,ram_data[DARWIN_GRIPPER_LEFT_BOT_ID],XL_TORQUE_LIMIT_L,force);
+    servo_mask|=1<<ram_data[DARWIN_GRIPPER_LEFT_TOP_ID];
+    servo_mask|=1<<ram_data[DARWIN_GRIPPER_LEFT_BOT_ID];
+    joint_motion_set_group_servos(1,servo_mask);
+  }
+  servo_mask=0x00000000;
+  if(ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID]!=0xFF && ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID]!=0xFF)
+  {
+    force=ram_data[DARWIN_GRIPPER_RIGHT_MAX_FORCE_L]+(ram_data[DARWIN_GRIPPER_RIGHT_MAX_FORCE_H]<<8);
+    dyn_master_write_word(&darwin_dyn_master_v2,ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID],XL_TORQUE_LIMIT_L,force);
+    dyn_master_write_word(&darwin_dyn_master_v2,ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID],XL_TORQUE_LIMIT_L,force);
+    servo_mask|=1<<ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID];
+    servo_mask|=1<<ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID];
+    joint_motion_set_group_servos(2,servo_mask);
+  }
+  grippers_close(GRIPPER_LEFT);
+  grippers_close(GRIPPER_RIGHT);
+}
+
+void grippers_set_period(uint16_t period_us)
+{
+  grippers_period=(period_us<<16)/1000000;
+  grippers_period_us=period_us;
+}
+
+uint16_t grippers_get_period(void)
+{
+  return grippers_period_us;
+}
+
+void grippers_open(grippers_t gripper)
+{
+  if(gripper==GRIPPER_LEFT)
+  {
+    ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_TOP_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_L];
+    ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_TOP_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_H];
+    ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_LEFT_TOP_ID]]=60;
+    ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_BOT_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_L];
+    ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_BOT_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_H];
+    ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_LEFT_BOT_ID]]=60;
+    joint_motion_start(1);
+  }
+  else if(gripper==GRIPPER_RIGHT)
+  {
+    ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_L];
+    ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_H];
+    ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID]]=60;
+    ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_L];
+    ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_H];
+    ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID]]=60;
+    joint_motion_start(2);
+  }
+}
+
+void grippers_close(grippers_t gripper)
+{
+  if(gripper==GRIPPER_LEFT)
+  {
+    ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_TOP_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_L];
+    ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_TOP_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_H];
+    ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_LEFT_TOP_ID]]=60;
+    ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_BOT_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_L];
+    ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_BOT_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_H];
+    ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_LEFT_BOT_ID]]=60;
+    joint_motion_start(1);
+  }
+  else if(gripper==GRIPPER_RIGHT)
+  {
+    ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_L];
+    ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_H];
+    ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID]]=60;
+    ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_L];
+    ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_H];
+    ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID]]=60;
+    joint_motion_start(2);
+  }
+}
+
+void grippers_stop(grippers_t gripper)
+{
+  if(gripper==GRIPPER_LEFT)
+    joint_motion_stop(1);
+  else
+    joint_motion_stop(2);
+}
+
+uint8_t gripper_is_open(grippers_t gripper)
+{
+  if(gripper==GRIPPER_LEFT)
+  {
+    if(ram_data[DARWIN_GRIPPER_CNTRL]&GRIPPER_OPENED_LEFT)
+      return 0x01;
+    else
+      return 0x00;
+  }
+  else
+  {
+    if(ram_data[DARWIN_GRIPPER_CNTRL]&GRIPPER_OPENED_RIGHT)
+      return 0x01;
+    else
+      return 0x00;
+  }
+}
+
+uint8_t gripper_is_close(grippers_t gripper)
+{
+  if(gripper==GRIPPER_LEFT)
+  {
+    if(ram_data[DARWIN_GRIPPER_CNTRL]&GRIPPER_OPENED_LEFT)
+      return 0x00;
+    else
+      return 0x01;
+  }
+  else
+  {
+    if(ram_data[DARWIN_GRIPPER_CNTRL]&GRIPPER_OPENED_RIGHT)
+      return 0x00;
+    else
+      return 0x01;
+  }
+}
+
+uint8_t gripper_is_moving(grippers_t gripper)
+{
+  if(gripper==GRIPPER_LEFT)
+    return are_joints_moving(1);
+  else
+    return are_joints_moving(2);
+}
+
+// operation functions
+uint8_t grippers_in_range(unsigned short int address, unsigned short int length)
+{
+  if(ram_in_window(GRIPPER_BASE_ADDRESS,GRIPPER_MEM_LENGTH,address,length) ||
+     ram_in_window(GRIPPER_EEPROM_ADDRESS,GRIPPER_EEPROM_LENGTH,address,length))
+    return 0x01;
+  else
+    return 0x00;
+}
+
+void grippers_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
+{
+
+}
+
+void grippers_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
+{
+  if(ram_in_range(DARWIN_GRIPPER_CNTRL,address,length))
+  {
+    if(data[DARWIN_GRIPPER_CNTRL-address]&GRIPPER_OPEN_RIGHT)
+      grippers_open(GRIPPER_RIGHT);
+    else if(data[DARWIN_GRIPPER_CNTRL-address]&GRIPPER_CLOSE_RIGHT)
+      grippers_close(GRIPPER_RIGHT);
+    if(data[DARWIN_GRIPPER_CNTRL-address]&GRIPPER_OPEN_LEFT)
+      grippers_open(GRIPPER_LEFT);
+    else if(data[DARWIN_GRIPPER_CNTRL-address]&GRIPPER_CLOSE_LEFT)
+      grippers_close(GRIPPER_LEFT);
+  }
+}
+
+void grippers_process(void)
+{
+  if(!are_joints_moving(1))
+  {
+    ram_data[DARWIN_GRIPPER_CNTRL]&=~GRIPPER_MOVING_LEFT;
+    if(gripper_left_target==0x01)
+      ram_data[DARWIN_GRIPPER_CNTRL]|=GRIPPER_OPENED_LEFT;
+    else
+      ram_data[DARWIN_GRIPPER_CNTRL]&=~GRIPPER_OPENED_LEFT;
+  }
+  else
+  {
+    ram_data[DARWIN_GRIPPER_CNTRL]&=~GRIPPER_OPENED_LEFT;
+    ram_data[DARWIN_GRIPPER_CNTRL]|=GRIPPER_MOVING_LEFT;
+  }
+  if(!are_joints_moving(2))
+  {
+    ram_data[DARWIN_GRIPPER_CNTRL]&=~GRIPPER_MOVING_RIGHT;
+    if(gripper_right_target==0x01)
+      ram_data[DARWIN_GRIPPER_CNTRL]|=GRIPPER_OPENED_RIGHT;
+    else
+      ram_data[DARWIN_GRIPPER_CNTRL]&=~GRIPPER_OPENED_RIGHT;
+  }
+  else
+  {
+    ram_data[DARWIN_GRIPPER_CNTRL]&=~GRIPPER_OPENED_RIGHT;
+    ram_data[DARWIN_GRIPPER_CNTRL]|=GRIPPER_MOVING_RIGHT;
+  }
+}

dynamixel_manager/src/modules/old/head_tracking.c

+#include "head_tracking.h"
+#include "ram.h"
+
+// private variables
+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_us)
+{
+  /* initialize private variables */
+  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;
+}
+
+void head_tracking_set_period(uint16_t period_us)
+{
+  head_tracking_period=(period_us<<16)/1000000;
+  head_tracking_period_us=period_us;
+}
+
+uint16_t head_tracking_get_period(void)
+{
+  return head_tracking_period_us;
+}
+
+void head_tracking_start(void)
+{
+  head_tracking_enabled=0x01;
+  ram_data[DARWIN_HEAD_CNTRL]|=HEAD_STATUS;
+}
+
+void head_tracking_stop(void)
+{
+  head_tracking_enabled=0x00;
+  ram_data[DARWIN_HEAD_CNTRL]&=(~HEAD_STATUS);
+}
+
+uint8_t head_is_tracking(void)
+{
+  return head_tracking_enabled;
+}
+
+void head_tracking_set_pan_range(int16_t max_angle,int16_t min_angle)
+{
+  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_get_pan_range(int16_t *max_angle,int16_t *min_angle)
+{
+  *max_angle=head_tracking_pan.max_angle>>9;
+  *min_angle=head_tracking_pan.min_angle>>9;
+}
+
+void head_tracking_set_pan_pid(uint16_t kp,uint16_t ki,uint16_t kd, uint16_t i_clamp)
+{
+  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_get_pan_pid(uint16_t *kp,uint16_t *ki,uint16_t *kd, uint16_t *i_clamp)
+{
+  *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_range(int16_t max_angle,int16_t min_angle)
+{
+  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_get_tilt_range(int16_t *max_angle,int16_t *min_angle)
+{
+  *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)
+{
+  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)
+{
+  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>>9);
+    if(ram_in_range(DARWIN_HEAD_PAN_TARGET_L,address,length))
+    { 
+      target=(target&0xFF00)|data[DARWIN_HEAD_PAN_TARGET_L-address];
+      ram_data[DARWIN_HEAD_PAN_TARGET_L]=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);
+      ram_data[DARWIN_HEAD_PAN_TARGET_H]=data[DARWIN_HEAD_PAN_TARGET_H-address];
+    }
+    head_tracking_pan.target_angle=(target<<9);
+  }
+  if(ram_in_window(DARWIN_HEAD_TILT_TARGET_L,2,address,length))
+  {
+    target=(head_tracking_tilt.target_angle>>9);
+    if(ram_in_range(DARWIN_HEAD_TILT_TARGET_L,address,length))
+    {
+      target=(target&0xFF00)|data[DARWIN_HEAD_TILT_TARGET_L-address];
+      ram_data[DARWIN_HEAD_TILT_TARGET_L]=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);
+      ram_data[DARWIN_HEAD_TILT_TARGET_H]=data[DARWIN_HEAD_TILT_TARGET_H-address];
+    }
+    head_tracking_tilt.target_angle=(target<<9);
+  }
+}
+
+// motion manager process function
+void head_tracking_process(void)
+{
+  int64_t pan_error, tilt_error;
+  int64_t derivative_pan=0, derivative_tilt=0;
+
+  if(head_tracking_enabled)
+  {
+    ram_data[DARWIN_HEAD_CNTRL]|=HEAD_STATUS;
+    if(manager_get_module(L_PAN)==MM_HEAD)
+    {
+      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(L_TILT)==MM_HEAD)
+    { 
+      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_data[DARWIN_HEAD_CNTRL]&=(~HEAD_STATUS);
+}
+

dynamixel_manager/src/modules/old/joint_motion.c

+#include "joint_motion.h"
+#include "ram.h"
+#include <stdlib.h>
+
+// private variables
+int64_t joint_target_angles[MANAGER_MAX_NUM_SERVOS];//48|16
+int64_t joint_target_speeds[MANAGER_MAX_NUM_SERVOS];//48|16
+int64_t joint_target_accels[MANAGER_MAX_NUM_SERVOS];//48|16
+int64_t joint_target_stop_angles[MANAGER_MAX_NUM_SERVOS];//48|16
+
+int64_t joint_current_angles[MANAGER_MAX_NUM_SERVOS];//48|16
+int64_t joint_current_speeds[MANAGER_MAX_NUM_SERVOS];//48|16
+
+int8_t joint_dir[MANAGER_MAX_NUM_SERVOS];// joint_direction
+
+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_us)
+{
+  uint8_t i;
+
+  /* initialize the internal variables */
+  for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
+  {
+    // target values
+    joint_target_angles[i]=manager_current_angles[i];
+    joint_target_speeds[i]=0;
+    joint_target_accels[i]=0;
+    // current values
+    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;
+  }
+  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;
+}
+
+void joint_motion_set_period(uint16_t period_us)
+{
+  joint_motion_period=(period_us<<16)/1000000;
+  joint_motion_period_us=period_us;
+}
+
+uint16_t joint_motion_get_period(void)
+{
+  return joint_motion_period_us;
+}
+
+void joint_motion_set_group_servos(uint8_t group_id,uint32_t servos)
+{
+  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;
+}
+
+uint32_t joint_motion_get_group_servos(uint8_t group_id)
+{
+  return joint_group_servos[group_id];
+}
+
+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,servo_index=0x00000001;i<MANAGER_MAX_NUM_SERVOS;i++,servo_index=servo_index<<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];
+        joint_current_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;
+          }
+        }
+        // stop angles
+        joint_target_stop_angles[i]=joint_target_speeds[i]*joint_target_speeds[i]/(2*joint_target_accels[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;
+      }
+    }
+  }
+  joint_moving[group_id]=0x01;
+  joint_stop[group_id]=0x00;
+  ram_data[DARWIN_JOINT_GRP0_CNTRL+group_id*5]|=JOINT_STATUS;
+}
+
+void joint_motion_stop(uint8_t group_id)
+{
+  joint_stop[group_id]=0x01;
+}
+
+uint8_t are_joints_moving(uint8_t group_id)
+{
+  return joint_moving[group_id];
+}
+
+// operation functions
+uint8_t joint_motion_in_range(unsigned short int address, unsigned short int length)
+{
+  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,k;
+  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,k=0;i<=DARWIN_JOINT_GRP0_SERVOS3+j*5;i++,k++)
+        if(ram_in_range(i,address,length))
+          servos|=(data[i-address]<<(k*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,i,j;
+  uint32_t servo_index;
+
+  for(j=0;j<4;j++)
+  {
+    moving=0x00;
+    if(joint_moving[j]==0x01)
+    {
+      for(i=0,servo_index=0x00000001;i<MANAGER_MAX_NUM_SERVOS;i++,servo_index=servo_index<<1)
+      {
+        if(manager_get_module(i)==MM_JOINTS && joint_group_servos[j]&servo_index)
+        {
+          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)
+            {
+              moving=0x01;
+              if(joint_current_speeds[i]!=joint_dir[i]*joint_target_speeds[i])// it is necessary to change the current speed
+              {
+                joint_current_angles[i]+=((joint_current_speeds[i]*joint_motion_period)>>17);
+                if(joint_current_speeds[i]>joint_dir[i]*joint_target_speeds[i])
+                {
+                  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)>>17);
+                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_stop_angles[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_current_angles[i]=joint_current_angles[i]+((joint_current_speeds[i]*joint_motion_period)>>17);
+                  joint_target_speeds[i]=joint_target_speeds[i]-((joint_target_accels[i]*joint_motion_period)>>16);
+                  if(joint_target_speeds[i]<0)
+                    joint_target_speeds[i]=0;
+                  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)>>17);
+                  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_current_speeds[i]=0;
+            }
+            manager_current_angles[i]=joint_current_angles[i];
+            manager_current_slopes[i]=5;
+          }
+        }
+      }
+      if(moving==0)
+      {
+        joint_moving[j]=0x00;
+        joint_stop[j]=0x00;
+        ram_data[DARWIN_JOINT_GRP0_CNTRL+j*5]&=(~JOINT_STATUS);
+      }
+    }
+  }
+}
+