From a668cb40cd07c9771aba7ddf16f312f1169591a8 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Tue, 11 Feb 2020 21:05:55 +0100
Subject: [PATCH] Added the old implementation of the motion modules.
---
.../include/modules/old/darwin_kinematics.h | 24 +
.../include/modules/old/darwin_math.h | 70 +++
.../include/modules/old/grippers.h | 34 +
.../include/modules/old/head_tracking.h | 41 ++
.../include/modules/old/joint_motion.h | 34 +
.../include/modules/old/stairs.h | 31 +
.../include/modules/old/walking.h | 30 +
.../src/modules/old/darwin_kinematics.c | 82 +++
.../src/modules/old/darwin_math.c | 237 +++++++
dynamixel_manager/src/modules/old/grippers.c | 218 +++++++
.../src/modules/old/head_tracking.c | 333 ++++++++++
.../src/modules/old/joint_motion.c | 285 +++++++++
dynamixel_manager/src/modules/old/stairs.c | 580 ++++++++++++++++++
dynamixel_manager/src/modules/old/walking.c | 515 ++++++++++++++++
14 files changed, 2514 insertions(+)
create mode 100755 dynamixel_manager/include/modules/old/darwin_kinematics.h
create mode 100755 dynamixel_manager/include/modules/old/darwin_math.h
create mode 100644 dynamixel_manager/include/modules/old/grippers.h
create mode 100644 dynamixel_manager/include/modules/old/head_tracking.h
create mode 100644 dynamixel_manager/include/modules/old/joint_motion.h
create mode 100755 dynamixel_manager/include/modules/old/stairs.h
create mode 100755 dynamixel_manager/include/modules/old/walking.h
create mode 100755 dynamixel_manager/src/modules/old/darwin_kinematics.c
create mode 100755 dynamixel_manager/src/modules/old/darwin_math.c
create mode 100644 dynamixel_manager/src/modules/old/grippers.c
create mode 100644 dynamixel_manager/src/modules/old/head_tracking.c
create mode 100644 dynamixel_manager/src/modules/old/joint_motion.c
create mode 100755 dynamixel_manager/src/modules/old/stairs.c
create mode 100755 dynamixel_manager/src/modules/old/walking.c
diff --git a/dynamixel_manager/include/modules/old/darwin_kinematics.h b/dynamixel_manager/include/modules/old/darwin_kinematics.h
new file mode 100755
index 0000000..783781e
--- /dev/null
+++ b/dynamixel_manager/include/modules/old/darwin_kinematics.h
@@ -0,0 +1,24 @@
+#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
diff --git a/dynamixel_manager/include/modules/old/darwin_math.h b/dynamixel_manager/include/modules/old/darwin_math.h
new file mode 100755
index 0000000..82ef927
--- /dev/null
+++ b/dynamixel_manager/include/modules/old/darwin_math.h
@@ -0,0 +1,70 @@
+#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
diff --git a/dynamixel_manager/include/modules/old/grippers.h b/dynamixel_manager/include/modules/old/grippers.h
new file mode 100644
index 0000000..b8818bd
--- /dev/null
+++ b/dynamixel_manager/include/modules/old/grippers.h
@@ -0,0 +1,34 @@
+#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
diff --git a/dynamixel_manager/include/modules/old/head_tracking.h b/dynamixel_manager/include/modules/old/head_tracking.h
new file mode 100644
index 0000000..f556264
--- /dev/null
+++ b/dynamixel_manager/include/modules/old/head_tracking.h
@@ -0,0 +1,41 @@
+#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
diff --git a/dynamixel_manager/include/modules/old/joint_motion.h b/dynamixel_manager/include/modules/old/joint_motion.h
new file mode 100644
index 0000000..f2f0e16
--- /dev/null
+++ b/dynamixel_manager/include/modules/old/joint_motion.h
@@ -0,0 +1,34 @@
+#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
diff --git a/dynamixel_manager/include/modules/old/stairs.h b/dynamixel_manager/include/modules/old/stairs.h
new file mode 100755
index 0000000..a34b4dc
--- /dev/null
+++ b/dynamixel_manager/include/modules/old/stairs.h
@@ -0,0 +1,31 @@
+#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
diff --git a/dynamixel_manager/include/modules/old/walking.h b/dynamixel_manager/include/modules/old/walking.h
new file mode 100755
index 0000000..30bc04e
--- /dev/null
+++ b/dynamixel_manager/include/modules/old/walking.h
@@ -0,0 +1,30 @@
+#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
diff --git a/dynamixel_manager/src/modules/old/darwin_kinematics.c b/dynamixel_manager/src/modules/old/darwin_kinematics.c
new file mode 100755
index 0000000..a60ca57
--- /dev/null
+++ b/dynamixel_manager/src/modules/old/darwin_kinematics.c
@@ -0,0 +1,82 @@
+#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;
+}
diff --git a/dynamixel_manager/src/modules/old/darwin_math.c b/dynamixel_manager/src/modules/old/darwin_math.c
new file mode 100755
index 0000000..e766463
--- /dev/null
+++ b/dynamixel_manager/src/modules/old/darwin_math.c
@@ -0,0 +1,237 @@
+#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];
+ }
+ }
+ }
+}
diff --git a/dynamixel_manager/src/modules/old/grippers.c b/dynamixel_manager/src/modules/old/grippers.c
new file mode 100644
index 0000000..14ab402
--- /dev/null
+++ b/dynamixel_manager/src/modules/old/grippers.c
@@ -0,0 +1,218 @@
+#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;
+ }
+}
diff --git a/dynamixel_manager/src/modules/old/head_tracking.c b/dynamixel_manager/src/modules/old/head_tracking.c
new file mode 100644
index 0000000..12ee27c
--- /dev/null
+++ b/dynamixel_manager/src/modules/old/head_tracking.c
@@ -0,0 +1,333 @@
+#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);
+}
+
diff --git a/dynamixel_manager/src/modules/old/joint_motion.c b/dynamixel_manager/src/modules/old/joint_motion.c
new file mode 100644
index 0000000..bd537d0
--- /dev/null
+++ b/dynamixel_manager/src/modules/old/joint_motion.c
@@ -0,0 +1,285 @@
+#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);
+ }
+ }
+ }
+}
+
diff --git a/dynamixel_manager/src/modules/old/stairs.c b/dynamixel_manager/src/modules/old/stairs.c
new file mode 100755
index 0000000..4255587
--- /dev/null
+++ b/dynamixel_manager/src/modules/old/stairs.c
@@ -0,0 +1,580 @@
+#include "stairs.h"
+#include "darwin_kinematics.h"
+#include "darwin_math.h"
+#include "ram.h"
+#include <math.h>
+
+typedef enum {SHIFT_WEIGHT_LEFT=0x10,RISE_RIGHT_FOOT=0x20,ADVANCE_RIGHT_FOOT=0x30,CONTACT_RIGHT_FOOT=0x40,SHIFT_WEIGHT_RIGHT=0x50,RISE_LEFT_FOOT=0x60,ADVANCE_LEFT_FOOT=0x70,CONTACT_LEFT_FOOT=0x80,CENTER_WEIGHT=0x90} stairs_phase_t;
+
+// internal motion variables
+float stairs_Z_stair_height;
+float stairs_Z_overshoot;
+float stairs_Y_shift_amplitude;
+float stairs_X_shift_amplitude;
+float stairs_R_shift_amplitude;
+float stairs_P_shift_amplitude;
+float stairs_A_shift_amplitude;
+float stairs_Y_spread_amplitude;
+float stairs_X_shift_body;
+
+// internal offset variables
+float stairs_Hip_Pitch_Offset;
+float stairs_X_Offset;
+float stairs_Y_Offset;
+float stairs_Z_Offset;
+float stairs_R_Offset;
+float stairs_P_Offset;
+float stairs_A_Offset;
+
+// internal time variables
+float stairs_Time;
+float stairs_period;
+float stairs_shift_weight_left_time;
+float stairs_rise_right_foot_time;
+float stairs_advance_right_foot_time;
+float stairs_contact_right_foot_time;
+float stairs_shift_weight_right_time;
+float stairs_rise_left_foot_time;
+float stairs_advance_left_foot_time;
+float stairs_contact_left_foot_time;
+float stairs_center_weight_time;
+
+// control variables
+uint8_t stairs_Ctrl_Running;
+uint8_t stairs_up;
+
+// private functions
+
+// public functions
+void stairs_init(uint16_t period_us)
+{
+ // initialize the internal motion variables
+
+
+ // initialize internal control variables
+ stairs_Time=0;
+ stairs_period=((float)period_us)/1000.0;
+ stairs_Ctrl_Running=0x00;
+ stairs_up=0x00;
+}
+
+void stairs_set_period(uint16_t period_us)
+{
+ stairs_period=((float)period_us)/1000.0;
+}
+
+uint16_t stairs_get_period(void)
+{
+ return (uint16_t)(stairs_period*1000);
+}
+
+void stairs_start(uint8_t up)
+{
+ // load all parameters
+ stairs_shift_weight_left_time=((float)ram_data[DARWIN_STAIRS_PHASE1_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE1_TIME_H]<<8));
+ stairs_rise_right_foot_time=((float)ram_data[DARWIN_STAIRS_PHASE2_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE2_TIME_H]<<8));
+ stairs_advance_right_foot_time=((float)ram_data[DARWIN_STAIRS_PHASE3_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE3_TIME_H]<<8));
+ stairs_contact_right_foot_time=((float)ram_data[DARWIN_STAIRS_PHASE4_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE4_TIME_H]<<8));
+ stairs_shift_weight_right_time=((float)ram_data[DARWIN_STAIRS_PHASE5_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE5_TIME_H]<<8));
+ stairs_rise_left_foot_time=((float)ram_data[DARWIN_STAIRS_PHASE6_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE6_TIME_H]<<8));
+ stairs_advance_left_foot_time=((float)ram_data[DARWIN_STAIRS_PHASE7_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE7_TIME_H]<<8));
+ stairs_contact_left_foot_time=((float)ram_data[DARWIN_STAIRS_PHASE8_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE8_TIME_H]<<8));
+ stairs_center_weight_time=((float)ram_data[DARWIN_STAIRS_PHASE9_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE9_TIME_H]<<8));
+ stairs_Z_stair_height=((float)((int8_t)ram_data[DARWIN_STAIRS_Z_HEIGHT]));
+ stairs_Z_overshoot=((float)((int8_t)ram_data[DARWIN_STAIRS_Z_OVERSHOOT]));
+ stairs_Y_shift_amplitude=((float)((int8_t)ram_data[DARWIN_STAIRS_Y_SHIFT]));
+ stairs_X_shift_amplitude=((float)((int8_t)ram_data[DARWIN_STAIRS_X_SHIFT]));
+ stairs_Hip_Pitch_Offset=((float)((int16_t)(ram_data[DARWIN_WALK_HIP_PITCH_OFF_L]+(ram_data[DARWIN_WALK_HIP_PITCH_OFF_H]<<8))))/1024.0;
+ stairs_R_shift_amplitude=((float)((int8_t)ram_data[DARWIN_STAIRS_R_SHIFT]))*PI/720.0;// (r_shift/4)*(pi/180)
+ stairs_P_shift_amplitude=((float)((int8_t)ram_data[DARWIN_STAIRS_P_SHIFT]))*PI/720.0;// (p_shift/4)*(pi/180)
+ stairs_A_shift_amplitude=((float)((int8_t)ram_data[DARWIN_STAIRS_A_SHIFT]))*PI/720.0;// (a_shift/4)*(pi/180)
+ stairs_Y_spread_amplitude=((float)((int8_t)ram_data[DARWIN_STAIRS_Y_SPREAD]));
+ stairs_X_shift_body=((float)((int8_t)ram_data[DARWIN_STAIRS_X_SHIFT_BODY]));
+ stairs_X_Offset=((float)((int8_t)ram_data[DARWIN_STAIRS_X_OFFSET]));
+ stairs_Y_Offset=((float)((int8_t)ram_data[DARWIN_STAIRS_Y_OFFSET]));
+ stairs_Z_Offset=((float)((int8_t)ram_data[DARWIN_STAIRS_Z_OFFSET]));
+ stairs_R_Offset=((float)((int8_t)ram_data[DARWIN_STAIRS_X_OFFSET]))*PI/1440.0;
+ stairs_P_Offset=((float)((int8_t)ram_data[DARWIN_STAIRS_Y_OFFSET]))*PI/1440.0;
+ stairs_A_Offset=((float)((int8_t)ram_data[DARWIN_STAIRS_Z_OFFSET]))*PI/1440.0;
+ // start operation
+ ram_data[DARWIN_STAIRS_CNTRL]|=STAIRS_STATUS;
+ stairs_Ctrl_Running=0x01;
+ stairs_up=up;
+}
+
+void stairs_stop(void)
+{
+ stairs_Ctrl_Running=0x00;
+}
+
+uint8_t is_climbing_stairs(void)
+{
+ if(ram_data[DARWIN_STAIRS_CNTRL]&STAIRS_STATUS)
+ return 0x01;
+ else
+ return 0x00;
+}
+
+uint8_t stairs_get_phase(void)
+{
+ return (int8_t)ram_data[DARWIN_STAIRS_CNTRL]&STAIRS_PHASE;
+}
+
+// motion manager interface functions
+void stairs_process(void)
+{
+ float angle[14]={0},ep[12]={0};
+ float delta;
+
+ if(ram_data[DARWIN_STAIRS_CNTRL]&STAIRS_STATUS)
+ {
+ ram_data[DARWIN_STAIRS_CNTRL]&=(~STAIRS_PHASE);
+ if(stairs_up)
+ {
+ if(stairs_Time>=0 && stairs_Time<=stairs_shift_weight_left_time)
+ {
+ //1
+ delta=stairs_Time/stairs_shift_weight_left_time;
+ ep[0]=stairs_X_Offset;
+ ep[1]=-stairs_Y_Offset/2.0+stairs_Y_shift_amplitude*delta;
+ ep[2]=stairs_Z_Offset;
+ ep[3]=stairs_R_Offset;
+ ep[4]=stairs_P_Offset;
+ ep[5]=stairs_A_Offset;
+ ep[6]=stairs_X_Offset;
+ ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude*delta;
+ ep[8]=stairs_Z_Offset;
+ ep[9]=stairs_R_Offset;
+ ep[10]=stairs_P_Offset;
+ ep[11]=stairs_A_Offset;
+ ram_data[DARWIN_STAIRS_CNTRL]|=SHIFT_WEIGHT_LEFT;
+ }
+ else if(stairs_Time>stairs_shift_weight_left_time && stairs_Time<=stairs_rise_right_foot_time)
+ {
+ //2
+ delta=(stairs_Time-stairs_shift_weight_left_time)/(stairs_rise_right_foot_time-stairs_shift_weight_left_time);
+ ep[0]=stairs_X_Offset;
+ ep[1]=-stairs_Y_Offset/2.0+stairs_Y_shift_amplitude;
+ ep[2]=stairs_Z_Offset;
+ ep[3]=stairs_R_Offset-stairs_R_shift_amplitude*delta;
+ ep[4]=stairs_P_Offset;
+ ep[5]=stairs_A_Offset;
+ ep[6]=stairs_X_Offset;
+ ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude;
+ ep[8]=stairs_Z_Offset+(stairs_Z_overshoot+stairs_Z_stair_height)*delta;
+ ep[9]=stairs_R_Offset-stairs_R_shift_amplitude*delta;
+ ep[10]=stairs_P_Offset;
+ ep[11]=stairs_A_Offset;
+ ram_data[DARWIN_STAIRS_CNTRL]|=RISE_RIGHT_FOOT;
+ }
+ else if(stairs_Time>stairs_rise_right_foot_time && stairs_Time<=stairs_advance_right_foot_time)
+ {
+ //3
+ delta=(stairs_Time-stairs_rise_right_foot_time)/(stairs_advance_right_foot_time-stairs_rise_right_foot_time);
+ ep[0]=stairs_X_Offset;
+ ep[1]=-stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0)*delta;
+ ep[2]=stairs_Z_Offset;
+ ep[3]=stairs_R_Offset-stairs_R_shift_amplitude;
+ ep[4]=stairs_P_Offset+stairs_P_shift_amplitude*delta;
+ ep[5]=stairs_A_Offset+stairs_A_shift_amplitude*delta;
+ ep[6]=stairs_X_Offset+stairs_X_shift_amplitude*delta;
+ ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0)*delta;
+ ep[8]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height;
+ ep[9]=stairs_R_Offset-stairs_R_shift_amplitude;
+ ep[10]=stairs_P_Offset+stairs_P_shift_amplitude*delta;
+ ep[11]=stairs_A_Offset+stairs_A_shift_amplitude*delta;
+ ram_data[DARWIN_STAIRS_CNTRL]|=ADVANCE_RIGHT_FOOT;
+ }
+ else if(stairs_Time>stairs_advance_right_foot_time && stairs_Time<=stairs_contact_right_foot_time)
+ {
+ //4
+ delta=(stairs_Time-stairs_advance_right_foot_time)/(stairs_contact_right_foot_time-stairs_advance_right_foot_time);
+ ep[0]=stairs_X_Offset;
+ ep[1]=-stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0);
+ ep[2]=stairs_Z_Offset;
+ ep[3]=stairs_R_Offset-stairs_R_shift_amplitude;
+ ep[4]=stairs_P_Offset+stairs_P_shift_amplitude;
+ ep[5]=stairs_A_Offset+stairs_A_shift_amplitude;
+ ep[6]=stairs_X_Offset+stairs_X_shift_amplitude;
+ ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0);
+ ep[8]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height-stairs_Z_overshoot*delta;
+ ep[9]=stairs_R_Offset-stairs_R_shift_amplitude;
+ ep[10]=stairs_P_Offset+stairs_P_shift_amplitude;
+ ep[11]=stairs_A_Offset+stairs_A_shift_amplitude;
+ ram_data[DARWIN_STAIRS_CNTRL]|=CONTACT_RIGHT_FOOT;
+ }
+ else if(stairs_Time>stairs_contact_right_foot_time && stairs_Time<=stairs_shift_weight_right_time)
+ {
+ //5
+ delta=(stairs_Time-stairs_contact_right_foot_time)/(stairs_shift_weight_right_time-stairs_contact_right_foot_time);
+ ep[0]=stairs_X_Offset-stairs_X_shift_body*delta;
+ ep[1]=-stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0)-(2*stairs_Y_shift_amplitude)*delta;
+ ep[2]=stairs_Z_Offset;
+ ep[3]=stairs_R_Offset-stairs_R_shift_amplitude+2.0*stairs_R_shift_amplitude*delta;
+ ep[4]=stairs_P_Offset+stairs_P_shift_amplitude-3.0*stairs_P_shift_amplitude*delta;
+ ep[5]=stairs_A_Offset+stairs_A_shift_amplitude;
+ ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body*delta;
+ ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0)-(2*stairs_Y_shift_amplitude)*delta;
+ ep[8]=stairs_Z_Offset+stairs_Z_stair_height;
+ ep[9]=stairs_R_Offset-stairs_R_shift_amplitude+2.0*stairs_R_shift_amplitude*delta;
+ ep[10]=stairs_P_Offset+stairs_P_shift_amplitude-3.0*stairs_P_shift_amplitude*delta;
+ ep[11]=stairs_A_Offset+stairs_A_shift_amplitude;
+ ram_data[DARWIN_STAIRS_CNTRL]|=SHIFT_WEIGHT_RIGHT;
+ }
+ else if(stairs_Time>stairs_shift_weight_right_time && stairs_Time<=stairs_rise_left_foot_time)
+ {
+ //6
+ delta=(stairs_Time-stairs_shift_weight_right_time)/(stairs_rise_left_foot_time-stairs_shift_weight_right_time);
+ ep[0]=stairs_X_Offset-stairs_X_shift_body;
+ ep[1]=-stairs_Y_Offset/2.0-stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0);
+ ep[2]=stairs_Z_Offset+(stairs_Z_overshoot+stairs_Z_stair_height)*delta;
+ ep[3]=stairs_R_Offset+stairs_R_shift_amplitude;
+ ep[4]=stairs_P_Offset-2.0*stairs_P_shift_amplitude;
+ ep[5]=stairs_A_Offset+stairs_A_shift_amplitude;
+ ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body;
+ ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0);
+ ep[8]=stairs_Z_Offset+stairs_Z_stair_height;
+ ep[9]=stairs_R_Offset+stairs_R_shift_amplitude;
+ ep[10]=stairs_P_Offset-2.0*stairs_P_shift_amplitude;
+ ep[11]=stairs_A_Offset+stairs_A_shift_amplitude;
+ ram_data[DARWIN_STAIRS_CNTRL]|=RISE_LEFT_FOOT;
+ }
+ else if(stairs_Time>stairs_rise_left_foot_time && stairs_Time<=stairs_advance_left_foot_time)
+ {
+ //7
+ delta=(stairs_Time-stairs_rise_left_foot_time)/(stairs_advance_left_foot_time-stairs_rise_left_foot_time);
+ ep[0]=stairs_X_Offset-stairs_X_shift_body+stairs_X_shift_body*delta;
+ ep[1]=-stairs_Y_Offset/2.0-stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0)+(stairs_Y_spread_amplitude/2.0)*delta;
+ ep[2]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height;
+ ep[3]=stairs_R_Offset+stairs_R_shift_amplitude;
+ ep[4]=stairs_P_Offset-2.0*stairs_P_shift_amplitude+stairs_P_shift_amplitude*delta;
+ ep[5]=stairs_A_Offset+stairs_A_shift_amplitude-stairs_A_shift_amplitude*delta;
+ ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body-(stairs_X_shift_amplitude-stairs_X_shift_body)*delta;
+ ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0)-(stairs_Y_spread_amplitude/2.0)*delta;
+ ep[8]=stairs_Z_Offset+stairs_Z_stair_height;
+ ep[9]=stairs_R_Offset+stairs_R_shift_amplitude;
+ ep[10]=stairs_P_Offset-2.0*stairs_P_shift_amplitude+stairs_P_shift_amplitude*delta;
+ ep[11]=stairs_A_Offset+stairs_A_shift_amplitude-stairs_A_shift_amplitude*delta;
+ ram_data[DARWIN_STAIRS_CNTRL]|=ADVANCE_LEFT_FOOT;
+ }
+ else if(stairs_Time>stairs_advance_left_foot_time && stairs_Time<=stairs_contact_left_foot_time)
+ {
+ //8
+ delta=(stairs_Time-stairs_advance_left_foot_time)/(stairs_contact_left_foot_time-stairs_advance_left_foot_time);
+ ep[0]=stairs_X_Offset;
+ ep[1]=-stairs_Y_Offset/2.0-stairs_Y_shift_amplitude;
+ ep[2]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height-stairs_Z_overshoot*delta;
+ ep[3]=stairs_R_Offset+stairs_R_shift_amplitude-stairs_R_shift_amplitude*delta;
+ ep[4]=stairs_P_Offset-stairs_P_shift_amplitude+stairs_P_shift_amplitude*delta;
+ ep[5]=stairs_A_Offset;
+ ep[6]=stairs_X_Offset;
+ ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude;
+ ep[8]=stairs_Z_Offset+stairs_Z_stair_height;
+ ep[9]=stairs_R_Offset+stairs_R_shift_amplitude-stairs_R_shift_amplitude*delta;
+ ep[10]=stairs_P_Offset-stairs_P_shift_amplitude+stairs_P_shift_amplitude*delta;
+ ep[11]=stairs_A_Offset;
+ ram_data[DARWIN_STAIRS_CNTRL]|=CONTACT_LEFT_FOOT;
+ }
+ else if(stairs_Time>stairs_contact_left_foot_time && stairs_Time<=stairs_center_weight_time)
+ {
+ //9
+ delta=(stairs_Time-stairs_contact_left_foot_time)/(stairs_center_weight_time-stairs_contact_left_foot_time);
+ ep[0]=stairs_X_Offset;
+ ep[1]=-stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+stairs_Y_shift_amplitude*delta;
+ ep[2]=stairs_Z_Offset+stairs_Z_stair_height-stairs_Z_stair_height*delta;
+ ep[3]=stairs_R_Offset;
+ ep[4]=stairs_P_Offset;
+ ep[5]=stairs_A_Offset;
+ ep[6]=stairs_X_Offset;
+ ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+stairs_Y_shift_amplitude*delta;
+ ep[8]=stairs_Z_Offset+stairs_Z_stair_height-stairs_Z_stair_height*delta;
+ ep[9]=stairs_R_Offset;
+ ep[10]=stairs_P_Offset;
+ ep[11]=stairs_A_Offset;
+ ram_data[DARWIN_STAIRS_CNTRL]|=CENTER_WEIGHT;
+ }
+ else
+ {
+ ram_data[DARWIN_STAIRS_CNTRL]&=(~STAIRS_STATUS);
+ stairs_Ctrl_Running=0x00;
+ }
+ }
+ else
+ {
+ if(stairs_Time>=0 && stairs_Time<=stairs_shift_weight_left_time)
+ {
+ //1
+ delta=stairs_Time/stairs_shift_weight_left_time;
+ ep[0]=stairs_X_Offset;
+ ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude*delta;
+ ep[2]=stairs_Z_Offset+stairs_Z_stair_height*delta;
+ ep[3]=stairs_R_Offset;
+ ep[4]=stairs_P_Offset;
+ ep[5]=stairs_A_Offset;
+ ep[6]=stairs_X_Offset;
+ ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude*delta;
+ ep[8]=stairs_Z_Offset+stairs_Z_stair_height*delta;
+ ep[9]=stairs_R_Offset;
+ ep[10]=stairs_P_Offset;
+ ep[11]=stairs_A_Offset;
+ ram_data[DARWIN_STAIRS_CNTRL]|=SHIFT_WEIGHT_LEFT;
+ }
+ else if(stairs_Time>stairs_shift_weight_left_time && stairs_Time<=stairs_rise_right_foot_time)
+ {
+ //2
+ delta=(stairs_Time-stairs_shift_weight_left_time)/(stairs_rise_right_foot_time-stairs_shift_weight_left_time);
+ ep[0]=stairs_X_Offset;
+ ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude;
+ ep[2]=stairs_Z_Offset+stairs_Z_stair_height;
+ ep[3]=stairs_R_Offset-stairs_R_shift_amplitude;
+ ep[4]=stairs_P_Offset;
+ ep[5]=stairs_A_Offset;
+ ep[6]=stairs_X_Offset;
+ ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude;
+ ep[8]=stairs_Z_Offset+stairs_Z_stair_height+stairs_Z_overshoot*delta;
+ ep[9]=stairs_R_Offset-stairs_R_shift_amplitude*delta;
+ ep[10]=stairs_P_Offset;
+ ep[11]=stairs_A_Offset;
+ ram_data[DARWIN_STAIRS_CNTRL]|=RISE_RIGHT_FOOT;
+ }
+ else if(stairs_Time>stairs_rise_right_foot_time && stairs_Time<=stairs_advance_right_foot_time)
+ {
+ //3
+ delta=(stairs_Time-stairs_rise_right_foot_time)/(stairs_advance_right_foot_time-stairs_rise_right_foot_time);
+ ep[0]=stairs_X_Offset;
+ ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0)*delta;
+ ep[2]=stairs_Z_Offset+stairs_Z_stair_height;
+ ep[3]=stairs_R_Offset-stairs_R_shift_amplitude;
+ ep[4]=stairs_P_Offset+stairs_P_shift_amplitude*delta;
+ ep[5]=stairs_A_Offset+stairs_A_shift_amplitude*delta;
+ ep[6]=stairs_X_Offset+stairs_X_shift_amplitude*delta;
+ ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0)*delta;
+ ep[8]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height;
+ ep[9]=stairs_R_Offset-stairs_R_shift_amplitude;
+ ep[10]=stairs_P_Offset+stairs_P_shift_amplitude*delta;
+ ep[11]=stairs_A_Offset+stairs_A_shift_amplitude*delta;
+ ram_data[DARWIN_STAIRS_CNTRL]|=ADVANCE_RIGHT_FOOT;
+ }
+ else if(stairs_Time>stairs_advance_right_foot_time && stairs_Time<=stairs_contact_right_foot_time)
+ {
+ //4
+ delta=(stairs_Time-stairs_advance_right_foot_time)/(stairs_contact_right_foot_time-stairs_advance_right_foot_time);
+ ep[0]=stairs_X_Offset;
+ ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0);
+ ep[2]=stairs_Z_Offset+stairs_Z_stair_height+stairs_Z_overshoot*delta;
+ ep[3]=stairs_R_Offset-stairs_R_shift_amplitude;
+ ep[4]=stairs_P_Offset+stairs_P_shift_amplitude;
+ ep[5]=stairs_A_Offset+stairs_A_shift_amplitude;
+ ep[6]=stairs_X_Offset+stairs_X_shift_amplitude;
+ ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0);
+ ep[8]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height-stairs_Z_stair_height*delta;
+ ep[9]=stairs_R_Offset-stairs_R_shift_amplitude;
+ ep[10]=stairs_P_Offset+stairs_P_shift_amplitude;
+ ep[11]=stairs_A_Offset+stairs_A_shift_amplitude;
+ ram_data[DARWIN_STAIRS_CNTRL]|=CONTACT_RIGHT_FOOT;
+ }
+ else if(stairs_Time>stairs_contact_right_foot_time && stairs_Time<=stairs_shift_weight_right_time)
+ {
+ //5
+ delta=(stairs_Time-stairs_contact_right_foot_time)/(stairs_shift_weight_right_time-stairs_contact_right_foot_time);
+ ep[0]=stairs_X_Offset-stairs_X_shift_body*delta;
+ ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0)-(2*stairs_Y_shift_amplitude)*delta;
+ ep[2]=stairs_Z_Offset+stairs_Z_stair_height+stairs_Z_overshoot;
+ ep[3]=stairs_R_Offset-stairs_R_shift_amplitude+2.0*stairs_R_shift_amplitude*delta;
+ ep[4]=stairs_P_Offset+stairs_P_shift_amplitude-(2.0*stairs_P_shift_amplitude)*delta;
+ ep[5]=stairs_A_Offset+stairs_A_shift_amplitude;
+ ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body*delta;
+ ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0)-(2*stairs_Y_shift_amplitude)*delta;
+ ep[8]=stairs_Z_Offset+stairs_Z_overshoot;
+ ep[9]=stairs_R_Offset-stairs_R_shift_amplitude+2.0*stairs_R_shift_amplitude*delta;
+ ep[10]=stairs_P_Offset+stairs_P_shift_amplitude-(2.0*stairs_P_shift_amplitude)*delta;
+ ep[11]=stairs_A_Offset+stairs_A_shift_amplitude;
+ ram_data[DARWIN_STAIRS_CNTRL]|=SHIFT_WEIGHT_RIGHT;
+ }
+ else if(stairs_Time>stairs_shift_weight_right_time && stairs_Time<=stairs_rise_left_foot_time)
+ {
+ //6
+ delta=(stairs_Time-stairs_shift_weight_right_time)/(stairs_rise_left_foot_time-stairs_shift_weight_right_time);
+ ep[0]=stairs_X_Offset-stairs_X_shift_body;
+ ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0);
+ ep[2]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height;
+ ep[3]=stairs_R_Offset+stairs_R_shift_amplitude;
+ ep[4]=stairs_P_Offset-1.0*stairs_P_shift_amplitude;
+ ep[5]=stairs_A_Offset+stairs_A_shift_amplitude;
+ ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body;
+ ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0);
+ ep[8]=stairs_Z_Offset+stairs_Z_overshoot-stairs_Z_overshoot*delta;
+ ep[9]=stairs_R_Offset+stairs_R_shift_amplitude;
+ ep[10]=stairs_P_Offset-1.0*stairs_P_shift_amplitude;
+ ep[11]=stairs_A_Offset+stairs_A_shift_amplitude;
+ ram_data[DARWIN_STAIRS_CNTRL]|=RISE_LEFT_FOOT;
+ }
+ else if(stairs_Time>stairs_rise_left_foot_time && stairs_Time<=stairs_advance_left_foot_time)
+ {
+ //7
+ delta=(stairs_Time-stairs_rise_left_foot_time)/(stairs_advance_left_foot_time-stairs_rise_left_foot_time);
+ ep[0]=stairs_X_Offset-stairs_X_shift_body+stairs_X_shift_body*delta;
+ ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0)+(stairs_Y_spread_amplitude/2.0)*delta;
+ ep[2]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height;
+ ep[3]=stairs_R_Offset+stairs_R_shift_amplitude;//+stairs_R_shift_amplitude*delta;
+ ep[4]=stairs_P_Offset-1.0*stairs_P_shift_amplitude+1.0*stairs_P_shift_amplitude*delta;
+ ep[5]=stairs_A_Offset+stairs_A_shift_amplitude-stairs_A_shift_amplitude*delta;
+ ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body-(stairs_X_shift_amplitude-stairs_X_shift_body)*delta;
+ ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0)-(stairs_Y_spread_amplitude/2.0)*delta;
+ ep[8]=stairs_Z_Offset;
+ ep[9]=stairs_R_Offset+stairs_R_shift_amplitude;//+stairs_R_shift_amplitude*delta;
+ ep[10]=stairs_P_Offset-1.0*stairs_P_shift_amplitude+1.0*stairs_P_shift_amplitude*delta;
+ ep[11]=stairs_A_Offset+stairs_A_shift_amplitude-stairs_A_shift_amplitude*delta;
+ ram_data[DARWIN_STAIRS_CNTRL]|=ADVANCE_LEFT_FOOT;
+ }
+ else if(stairs_Time>stairs_advance_left_foot_time && stairs_Time<=stairs_contact_left_foot_time)
+ {
+ //8
+ delta=(stairs_Time-stairs_advance_left_foot_time)/(stairs_contact_left_foot_time-stairs_advance_left_foot_time);
+ ep[0]=stairs_X_Offset;
+ ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude;
+ ep[2]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height-(stairs_Z_stair_height+stairs_Z_overshoot)*delta;
+ ep[3]=stairs_R_Offset+stairs_R_shift_amplitude-stairs_R_shift_amplitude*delta;
+ ep[4]=stairs_P_Offset;
+ ep[5]=stairs_A_Offset;
+ ep[6]=stairs_X_Offset;
+ ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude;
+ ep[8]=stairs_Z_Offset;
+ ep[9]=stairs_R_Offset+stairs_R_shift_amplitude-stairs_R_shift_amplitude*delta;
+ ep[10]=stairs_P_Offset;
+ ep[11]=stairs_A_Offset;
+ ram_data[DARWIN_STAIRS_CNTRL]|=CONTACT_LEFT_FOOT;
+ }
+ else if(stairs_Time>stairs_contact_left_foot_time && stairs_Time<=stairs_center_weight_time)
+ {
+ //9
+ delta=(stairs_Time-stairs_contact_left_foot_time)/(stairs_center_weight_time-stairs_contact_left_foot_time);
+ ep[0]=stairs_X_Offset;
+ ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+stairs_Y_shift_amplitude*delta;
+ ep[2]=stairs_Z_Offset;
+ ep[3]=stairs_R_Offset+stairs_R_shift_amplitude-stairs_R_shift_amplitude*delta;
+ ep[4]=stairs_P_Offset;
+ ep[5]=stairs_A_Offset;
+ ep[6]=stairs_X_Offset;
+ ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+stairs_Y_shift_amplitude*delta;
+ ep[8]=stairs_Z_Offset;
+ ep[9]=stairs_R_Offset+stairs_R_shift_amplitude-stairs_R_shift_amplitude*delta;
+ ep[10]=stairs_P_Offset;
+ ep[11]=stairs_A_Offset;
+ ram_data[DARWIN_STAIRS_CNTRL]|=CENTER_WEIGHT;
+ }
+ else
+ {
+ ram_data[DARWIN_STAIRS_CNTRL]&=(~STAIRS_STATUS);
+ stairs_Ctrl_Running=0x00;
+ }
+ }
+
+ if(ram_data[DARWIN_STAIRS_CNTRL]&STAIRS_STATUS)
+ stairs_Time += stairs_period;
+ else
+ stairs_Time=0;
+
+ // Compute angles with the inverse kinematics
+ if((darwin_leg_ik(&angle[0],ep[0],ep[1],ep[2],ep[3],ep[4],ep[5])!=1) ||
+ (darwin_leg_ik(&angle[6],ep[6],ep[7],ep[8],ep[9],ep[10],ep[11])!=1))
+ return;// Do not use angles
+
+ // Compute motor value
+ if(manager_get_module(R_HIP_YAW)==MM_WALKING)
+ {
+ manager_current_angles[R_HIP_YAW]=((-180.0*angle[0])/PI)*65536.0;
+ manager_current_slopes[R_HIP_YAW]=5;
+ }
+ if(manager_get_module(R_HIP_ROLL)==MM_WALKING)
+ {
+ manager_current_angles[R_HIP_ROLL]=((-180.0*angle[1])/PI)*65536.0;
+ manager_current_slopes[R_HIP_ROLL]=5;
+ }
+ if(manager_get_module(R_HIP_PITCH)==MM_WALKING)
+ {
+ manager_current_angles[R_HIP_PITCH]=((180.0*angle[2])/PI-stairs_Hip_Pitch_Offset)*65536.0;
+ manager_current_slopes[R_HIP_PITCH]=5;
+ }
+ if(manager_get_module(R_KNEE)==MM_WALKING)
+ {
+ manager_current_angles[R_KNEE]=((180.0*angle[3])/PI)*65536.0;
+ manager_current_slopes[R_KNEE]=5;
+ }
+ if(manager_get_module(R_ANKLE_PITCH)==MM_WALKING)
+ {
+ manager_current_angles[R_ANKLE_PITCH]=((-180.0*angle[4])/PI)*65536.0;
+ manager_current_slopes[R_ANKLE_PITCH]=5;
+ }
+ if(manager_get_module(R_ANKLE_ROLL)==MM_WALKING)
+ {
+ manager_current_angles[R_ANKLE_ROLL]=((180.0*angle[5])/PI)*65536.0;
+ manager_current_slopes[R_ANKLE_ROLL]=5;
+ }
+ if(manager_get_module(L_HIP_YAW)==MM_WALKING)
+ {
+ manager_current_angles[L_HIP_YAW]=((-180.0*angle[6])/PI)*65536.0;
+ manager_current_slopes[L_HIP_YAW]=5;
+ }
+ if(manager_get_module(L_HIP_ROLL)==MM_WALKING)
+ {
+ manager_current_angles[L_HIP_ROLL]=((-180.0*angle[7])/PI)*65536.0;
+ manager_current_slopes[L_HIP_ROLL]=5;
+ }
+ if(manager_get_module(L_HIP_PITCH)==MM_WALKING)
+ {
+ manager_current_angles[L_HIP_PITCH]=((-180.0*angle[8])/PI+stairs_Hip_Pitch_Offset)*65536.0;
+ manager_current_slopes[L_HIP_PITCH]=5;
+ }
+ if(manager_get_module(L_KNEE)==MM_WALKING)
+ {
+ manager_current_angles[L_KNEE]=((-180.0*angle[9])/PI)*65536.0;
+ manager_current_slopes[L_KNEE]=5;
+ }
+ if(manager_get_module(L_ANKLE_PITCH)==MM_WALKING)
+ {
+ manager_current_angles[L_ANKLE_PITCH]=((180.0*angle[10])/PI)*65536.0;
+ manager_current_slopes[L_ANKLE_PITCH]=5;
+ }
+ if(manager_get_module(L_ANKLE_ROLL)==MM_WALKING)
+ {
+ manager_current_angles[L_ANKLE_ROLL]=((180.0*angle[11])/PI)*65536.0;
+ manager_current_slopes[L_ANKLE_ROLL]=5;
+ }
+ }
+}
+
+// operation functions
+uint8_t stairs_in_range(unsigned short int address, unsigned short int length)
+{
+ if(ram_in_window(STAIRS_BASE_ADDRESS,STAIRS_MEM_LENGTH,address,length) ||
+ ram_in_window(STAIRS_EEPROM_ADDRESS,STAIRS_EEPROM_LENGTH,address,length))
+ return 0x01;
+ else
+ return 0x00;
+}
+
+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)
+{
+ uint16_t i;
+
+ // walk control
+ if(ram_in_range(DARWIN_STAIRS_CNTRL,address,length))
+ {
+ if(data[DARWIN_STAIRS_CNTRL-address]&STAIRS_START_UP)
+ stairs_start(0x01);
+ if(data[DARWIN_STAIRS_CNTRL-address]&STAIRS_START_DOWN)
+ stairs_start(0x00);
+ if(data[DARWIN_STAIRS_CNTRL-address]&STAIRS_STOP)
+ stairs_stop();
+ }
+ for(i=STAIRS_EEPROM_ADDRESS;i<=STAIRS_EEPROM_ADDRESS+STAIRS_EEPROM_LENGTH;i++)
+ if(ram_in_range(i,address,length))
+ ram_data[i]=data[i-address];
+}
+
diff --git a/dynamixel_manager/src/modules/old/walking.c b/dynamixel_manager/src/modules/old/walking.c
new file mode 100755
index 0000000..57277de
--- /dev/null
+++ b/dynamixel_manager/src/modules/old/walking.c
@@ -0,0 +1,515 @@
+#include "walking.h"
+#include "darwin_kinematics.h"
+#include "darwin_math.h"
+#include "ram.h"
+#include <math.h>
+
+enum {PHASE0=0x00,PHASE1=0x40,PHASE2=0x80,PHASE3=0xC0};
+
+// Walking control
+uint8_t A_MOVE_AIM_ON=0x00;
+
+// internal motion variables
+float m_X_Swap_Phase_Shift;
+float m_X_Swap_Amplitude;
+float m_X_Swap_Amplitude_Shift;
+float m_X_Move_Phase_Shift;
+float m_X_Move_Amplitude;
+float m_X_Move_Amplitude_Shift;
+float m_Y_Swap_Phase_Shift;
+float m_Y_Swap_Amplitude;
+float m_Y_Swap_Amplitude_Shift;
+float m_Y_Move_Phase_Shift;
+float m_Y_Move_Amplitude;
+float m_Y_Move_Amplitude_Shift;
+float m_Z_Swap_Phase_Shift;
+float m_Z_Swap_Amplitude;
+float m_Z_Swap_Amplitude_Shift;
+float m_Z_Move_Phase_Shift;
+float m_Z_Move_Amplitude;
+float m_Z_Move_Amplitude_Shift;
+float m_A_Move_Phase_Shift;
+float m_A_Move_Amplitude;
+float m_A_Move_Amplitude_Shift;
+
+// internal offset variables
+float m_Hip_Pitch_Offset;
+float m_Pelvis_Offset;
+float m_Pelvis_Swing;
+float m_Arm_Swing_Gain;
+float m_X_Offset;
+float m_Y_Offset;
+float m_Z_Offset;
+float m_R_Offset;
+float m_P_Offset;
+float m_A_Offset;
+
+// internal time variables
+float m_Time;
+float m_PeriodTime;
+float m_X_Swap_PeriodTime;
+float m_X_Move_PeriodTime;
+float m_Y_Swap_PeriodTime;
+float m_Y_Move_PeriodTime;
+float m_Z_Swap_PeriodTime;
+float m_Z_Move_PeriodTime;
+float m_A_Move_PeriodTime;
+float m_SSP_Time_Start_L;
+float m_SSP_Time_End_L;
+float m_SSP_Time_Start_R;
+float m_SSP_Time_End_R;
+float m_Phase_Time1;
+float m_Phase_Time2;
+float m_Phase_Time3;
+
+// control variables
+uint8_t m_Ctrl_Running;
+float walking_period;
+
+// private functions
+float wsin(float time, float period, float period_shift, float mag, float mag_shift)
+{
+ return mag*sin(((2.0*PI*time)/period)-period_shift)+mag_shift;
+}
+
+void update_param_time()
+{
+ float m_SSP_Ratio;
+ float m_SSP_Time;
+
+ m_PeriodTime=((float)((int16_t)(ram_data[DARWIN_WALK_PERIOD_TIME_L]+(ram_data[DARWIN_WALK_PERIOD_TIME_H]<<8))));
+ m_SSP_Ratio=1.0-((float)ram_data[DARWIN_WALK_DSP_RATIO])/256.0;
+
+ m_SSP_Time=m_PeriodTime*m_SSP_Ratio;
+
+ m_X_Swap_PeriodTime=m_PeriodTime/2.0;
+ m_X_Move_PeriodTime=m_SSP_Time;
+ m_Y_Swap_PeriodTime=m_PeriodTime;
+ m_Y_Move_PeriodTime=m_SSP_Time;
+ m_Z_Swap_PeriodTime=m_PeriodTime/2.0;
+ m_Z_Move_PeriodTime=m_SSP_Time/2.0;
+ m_A_Move_PeriodTime=m_SSP_Time;
+
+ m_SSP_Time_Start_L=(1-m_SSP_Ratio)*m_PeriodTime/4.0;
+ m_SSP_Time_End_L=(1+m_SSP_Ratio)*m_PeriodTime/4.0;
+ m_SSP_Time_Start_R=(3-m_SSP_Ratio)*m_PeriodTime/4.0;
+ m_SSP_Time_End_R=(3+m_SSP_Ratio)*m_PeriodTime/4.0;
+
+ m_Phase_Time1=(m_SSP_Time_End_L+m_SSP_Time_Start_L)/2.0;
+ m_Phase_Time2=(m_SSP_Time_Start_R+m_SSP_Time_End_L)/2.0;
+ m_Phase_Time3=(m_SSP_Time_End_R+m_SSP_Time_Start_R)/2.0;
+
+ // m_pelvis_offset and m_pelvis_Swing in degrees
+ m_Pelvis_Offset=((float)ram_data[DARWIN_WALK_PELVIS_OFFSET])*PI/1440.0;
+ m_Pelvis_Swing=m_Pelvis_Offset*0.35;
+ m_Arm_Swing_Gain=((float)ram_data[DARWIN_WALK_ARM_SWING_GAIN])/32.0;
+}
+
+void update_param_move()
+{
+ float target_x_amplitude;
+ float target_y_amplitude;
+ float target_a_amplitude;
+ static float current_x_amplitude=0;
+ static float current_y_amplitude=0;
+ static float current_a_amplitude=0;
+
+ target_x_amplitude=((float)((int8_t)ram_data[DARWIN_WALK_STEP_FW_BW]));
+ // change longitudinal and transversal velocities to get to the target ones
+ if(current_x_amplitude<target_x_amplitude)
+ {
+ current_x_amplitude+=(((float)ram_data[DARWIN_WALK_MAX_VEL])*m_PeriodTime)/1000.0;
+ if(current_x_amplitude>target_x_amplitude)
+ current_x_amplitude=target_x_amplitude;
+ }
+ else if(current_x_amplitude>target_x_amplitude)
+ {
+ current_x_amplitude-=(((float)ram_data[DARWIN_WALK_MAX_VEL])*m_PeriodTime)/1000.0;
+ if(current_x_amplitude<target_x_amplitude)
+ current_x_amplitude=target_x_amplitude;
+ }
+ m_X_Move_Amplitude=current_x_amplitude;
+ m_X_Swap_Amplitude=current_x_amplitude*((float)ram_data[DARWIN_WALK_STEP_FW_BW_RATIO])/256.0;
+
+ // Right/Left
+ target_y_amplitude=((float)((int8_t)ram_data[DARWIN_WALK_STEP_LEFT_RIGHT]));
+ if(current_y_amplitude<target_y_amplitude)
+ {
+ current_y_amplitude+=(((float)ram_data[DARWIN_WALK_MAX_VEL])*m_PeriodTime)/1000.0;
+ if(current_y_amplitude>target_y_amplitude)
+ current_y_amplitude=target_y_amplitude;
+ }
+ else if(current_y_amplitude>target_y_amplitude)
+ {
+ current_y_amplitude-=(((float)ram_data[DARWIN_WALK_MAX_VEL])*m_PeriodTime)/1000.0;
+ if(current_y_amplitude<target_y_amplitude)
+ current_y_amplitude=target_y_amplitude;
+ }
+ m_Y_Move_Amplitude=current_y_amplitude/2.0;
+ if(m_Y_Move_Amplitude>0)
+ m_Y_Move_Amplitude_Shift=m_Y_Move_Amplitude;
+ else
+ m_Y_Move_Amplitude_Shift=-m_Y_Move_Amplitude;
+ m_Y_Swap_Amplitude=((float)ram_data[DARWIN_WALK_SWING_RIGHT_LEFT])+m_Y_Move_Amplitude_Shift*0.04;
+
+ m_Z_Move_Amplitude=((float)ram_data[DARWIN_WALK_FOOT_HEIGHT])/2.0;
+ m_Z_Move_Amplitude_Shift=m_Z_Move_Amplitude / 2.0;
+ m_Z_Swap_Amplitude=((float)ram_data[DARWIN_WALK_SWING_TOP_DOWN]);
+ m_Z_Swap_Amplitude_Shift=m_Z_Swap_Amplitude;
+
+ // Direction
+ target_a_amplitude=((float)((int8_t)ram_data[DARWIN_WALK_STEP_DIRECTION]))/8.0;
+ if(current_a_amplitude<target_a_amplitude)
+ {
+ current_a_amplitude+=(((float)ram_data[DARWIN_WALK_MAX_ROT_VEL])*m_PeriodTime)/8000.0;
+ if(current_a_amplitude>target_a_amplitude)
+ current_a_amplitude=target_a_amplitude;
+ }
+ else if(current_a_amplitude>target_a_amplitude)
+ {
+ current_a_amplitude-=(((float)ram_data[DARWIN_WALK_MAX_ROT_VEL])*m_PeriodTime)/8000.0;
+ if(current_a_amplitude<target_a_amplitude)
+ current_a_amplitude=target_a_amplitude;
+ }
+
+ if(A_MOVE_AIM_ON==0x00)
+ {
+ m_A_Move_Amplitude=current_a_amplitude*PI/180.0/2.0;
+ if(m_A_Move_Amplitude>0)
+ m_A_Move_Amplitude_Shift=m_A_Move_Amplitude;
+ else
+ m_A_Move_Amplitude_Shift=-m_A_Move_Amplitude;
+ }
+ else
+ {
+ m_A_Move_Amplitude=-current_a_amplitude*PI/180.0/2.0;
+ if(m_A_Move_Amplitude>0)
+ m_A_Move_Amplitude_Shift=-m_A_Move_Amplitude;
+ else
+ m_A_Move_Amplitude_Shift=m_A_Move_Amplitude;
+ }
+}
+
+void update_param_balance()
+{
+ m_X_Offset = ((float)((int8_t)ram_data[DARWIN_WALK_X_OFFSET]));
+ m_Y_Offset = ((float)((int8_t)ram_data[DARWIN_WALK_Y_OFFSET]));
+ m_Z_Offset = ((float)((int8_t)ram_data[DARWIN_WALK_Z_OFFSET]));
+ m_R_Offset = ((float)((int8_t)ram_data[DARWIN_WALK_ROLL_OFFSET]))*PI/1440.0;// (r_offset/8)*(pi/180)
+ m_P_Offset = ((float)((int8_t)ram_data[DARWIN_WALK_PITCH_OFFSET]))*PI/1440.0;// (p_offset/8)*(pi/180)
+ m_A_Offset = ((float)((int8_t)ram_data[DARWIN_WALK_YAW_OFFSET]))*PI/1440.0;// (a_offset/8)*(pi/180)
+ m_Hip_Pitch_Offset = ((float)((int16_t)(ram_data[DARWIN_WALK_HIP_PITCH_OFF_L]+(ram_data[DARWIN_WALK_HIP_PITCH_OFF_H]<<8))))/1024.0;
+}
+
+// public functions
+void walking_init(uint16_t period_us)
+{
+ // initialize the internal motion variables
+ m_X_Swap_Phase_Shift=PI;
+ m_X_Swap_Amplitude_Shift=0.0;
+ m_X_Move_Phase_Shift=PI/2.0;
+ m_X_Move_Amplitude=0.0;
+ m_X_Move_Amplitude_Shift=0.0;
+ m_Y_Swap_Phase_Shift=0.0;
+ m_Y_Swap_Amplitude_Shift=0.0;
+ m_Y_Move_Phase_Shift=PI/2.0;
+ m_Z_Swap_Phase_Shift=PI*3.0/2.0;
+ m_Z_Move_Phase_Shift=PI/2.0;
+ m_A_Move_Phase_Shift=PI/2.0;
+
+ // initialize internal control variables
+ m_Time=0;
+ walking_period=((float)period_us)/1000.0;
+ m_Ctrl_Running=0x00;
+
+ update_param_time();
+ update_param_move();
+
+ walking_process();
+}
+
+void walking_set_period(uint16_t period_us)
+{
+ walking_period=((float)period_us)/1000.0;
+}
+
+uint16_t walking_get_period(void)
+{
+ return (uint16_t)(walking_period*1000);
+}
+
+void walking_start(void)
+{
+ ram_data[DARWIN_WALK_CNTRL]|=WALK_STATUS;
+ m_Ctrl_Running=0x01;
+}
+
+void walking_stop(void)
+{
+ m_Ctrl_Running=0x00;
+}
+
+uint8_t is_walking(void)
+{
+ if(ram_data[DARWIN_WALK_CNTRL]&WALK_STATUS)
+ return 0x01;
+ else
+ return 0x00;
+}
+
+// motion manager interface functions
+void walking_process(void)
+{
+ float x_swap,y_swap,z_swap,a_swap,b_swap,c_swap;
+ float x_move_r,y_move_r,z_move_r,a_move_r,b_move_r,c_move_r;
+ float x_move_l,y_move_l,z_move_l,a_move_l,b_move_l,c_move_l;
+ float pelvis_offset_r,pelvis_offset_l;
+// float m_Body_Swing_Y,m_Body_Swing_Z;
+ float angle[14],ep[12];
+
+ if(m_Time==0)
+ {
+ update_param_time();
+ ram_data[DARWIN_WALK_CNTRL]&=(~WALK_PHASE);
+ ram_data[DARWIN_WALK_CNTRL]|=PHASE0;
+ if(m_Ctrl_Running==0x00)
+ {
+ if(m_X_Move_Amplitude==0 && m_Y_Move_Amplitude==0 && m_A_Move_Amplitude==0)
+ ram_data[DARWIN_WALK_CNTRL]&=(~WALK_STATUS);
+ else
+ {
+ ram_data[DARWIN_WALK_STEP_FW_BW]=0.0;
+ ram_data[DARWIN_WALK_STEP_LEFT_RIGHT]=0.0;
+ ram_data[DARWIN_WALK_STEP_DIRECTION]=0.0;
+ }
+ }
+ }
+ else if(m_Time>=(m_Phase_Time1-walking_period/2.0) && m_Time<(m_Phase_Time1+walking_period/2.0))
+ {
+ update_param_move();
+ ram_data[DARWIN_WALK_CNTRL]&=(~WALK_PHASE);
+ ram_data[DARWIN_WALK_CNTRL]|=PHASE1;
+ }
+ else if(m_Time>=(m_Phase_Time2-walking_period/2.0) && m_Time<(m_Phase_Time2+walking_period/2.0))
+ {
+ update_param_time();
+ m_Time=m_Phase_Time2;
+ ram_data[DARWIN_WALK_CNTRL]&=(~WALK_PHASE);
+ ram_data[DARWIN_WALK_CNTRL]|=PHASE2;
+ if(m_Ctrl_Running==0x00)
+ {
+ if(m_X_Move_Amplitude==0 && m_Y_Move_Amplitude==0 && m_A_Move_Amplitude==0)
+ ram_data[DARWIN_WALK_CNTRL]&=~WALK_STATUS;
+ else
+ {
+ ram_data[DARWIN_WALK_STEP_FW_BW]=0.0;
+ ram_data[DARWIN_WALK_STEP_LEFT_RIGHT]=0.0;
+ ram_data[DARWIN_WALK_STEP_DIRECTION]=0.0;
+ }
+ }
+ }
+ else if(m_Time>=(m_Phase_Time3-walking_period/2.0) && m_Time<(m_Phase_Time3+walking_period/2.0))
+ {
+ update_param_move();
+ ram_data[DARWIN_WALK_CNTRL]&=(~WALK_PHASE);
+ ram_data[DARWIN_WALK_CNTRL]|=PHASE3;
+ }
+ update_param_balance();
+
+ // Compute endpoints
+ x_swap=wsin(m_Time,m_X_Swap_PeriodTime,m_X_Swap_Phase_Shift,m_X_Swap_Amplitude,m_X_Swap_Amplitude_Shift);
+ y_swap=wsin(m_Time,m_Y_Swap_PeriodTime,m_Y_Swap_Phase_Shift,m_Y_Swap_Amplitude,m_Y_Swap_Amplitude_Shift);
+ z_swap=wsin(m_Time,m_Z_Swap_PeriodTime,m_Z_Swap_Phase_Shift,m_Z_Swap_Amplitude,m_Z_Swap_Amplitude_Shift);
+ a_swap=0;
+ b_swap=0;
+ c_swap=0;
+ if(m_Time<=m_SSP_Time_Start_L)
+ {
+ x_move_l=wsin(m_SSP_Time_Start_L,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_L,m_X_Move_Amplitude,m_X_Move_Amplitude_Shift);
+ y_move_l=wsin(m_SSP_Time_Start_L,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_L,m_Y_Move_Amplitude,m_Y_Move_Amplitude_Shift);
+ z_move_l=wsin(m_SSP_Time_Start_L,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_L,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift);
+ c_move_l=wsin(m_SSP_Time_Start_L,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_L,m_A_Move_Amplitude,m_A_Move_Amplitude_Shift);
+ x_move_r=wsin(m_SSP_Time_Start_L,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_L,-m_X_Move_Amplitude,-m_X_Move_Amplitude_Shift);
+ y_move_r=wsin(m_SSP_Time_Start_L,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_L,-m_Y_Move_Amplitude,-m_Y_Move_Amplitude_Shift);
+ z_move_r=wsin(m_SSP_Time_Start_R,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_R,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift);
+ c_move_r=wsin(m_SSP_Time_Start_L,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_L,-m_A_Move_Amplitude,-m_A_Move_Amplitude_Shift);
+ pelvis_offset_l=0;
+ pelvis_offset_r=0;
+ }
+ else if(m_Time<=m_SSP_Time_End_L)
+ {
+ x_move_l=wsin(m_Time,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_L,m_X_Move_Amplitude,m_X_Move_Amplitude_Shift);
+ y_move_l=wsin(m_Time,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_L,m_Y_Move_Amplitude,m_Y_Move_Amplitude_Shift);
+ z_move_l=wsin(m_Time,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_L,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift);
+ c_move_l=wsin(m_Time,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_L,m_A_Move_Amplitude,m_A_Move_Amplitude_Shift);
+ x_move_r=wsin(m_Time,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_L,-m_X_Move_Amplitude,-m_X_Move_Amplitude_Shift);
+ y_move_r=wsin(m_Time,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_L,-m_Y_Move_Amplitude,-m_Y_Move_Amplitude_Shift);
+ z_move_r=wsin(m_SSP_Time_Start_R,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_R,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift);
+ c_move_r=wsin(m_Time,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_L,-m_A_Move_Amplitude,-m_A_Move_Amplitude_Shift);
+ pelvis_offset_l=wsin(m_Time,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_L,m_Pelvis_Swing/2.0,m_Pelvis_Swing/2.0);
+ pelvis_offset_r=wsin(m_Time,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_L,-m_Pelvis_Offset/2.0,-m_Pelvis_Offset/2.0);
+ }
+ else if(m_Time<=m_SSP_Time_Start_R)
+ {
+ x_move_l=wsin(m_SSP_Time_End_L,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_L,m_X_Move_Amplitude,m_X_Move_Amplitude_Shift);
+ y_move_l=wsin(m_SSP_Time_End_L,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_L,m_Y_Move_Amplitude,m_Y_Move_Amplitude_Shift);
+ z_move_l=wsin(m_SSP_Time_End_L,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_L,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift);
+ c_move_l=wsin(m_SSP_Time_End_L,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_L,m_A_Move_Amplitude,m_A_Move_Amplitude_Shift);
+ x_move_r=wsin(m_SSP_Time_End_L,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_L,-m_X_Move_Amplitude,-m_X_Move_Amplitude_Shift);
+ y_move_r=wsin(m_SSP_Time_End_L,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_L,-m_Y_Move_Amplitude,-m_Y_Move_Amplitude_Shift);
+ z_move_r=wsin(m_SSP_Time_Start_R,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_R,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift);
+ c_move_r=wsin(m_SSP_Time_End_L,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_L,-m_A_Move_Amplitude,-m_A_Move_Amplitude_Shift);
+ pelvis_offset_l=0.0;
+ pelvis_offset_r=0.0;
+ }
+ else if(m_Time<=m_SSP_Time_End_R)
+ {
+ x_move_l=wsin(m_Time,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_R+PI,m_X_Move_Amplitude,m_X_Move_Amplitude_Shift);
+ y_move_l=wsin(m_Time,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_R+PI,m_Y_Move_Amplitude,m_Y_Move_Amplitude_Shift);
+ z_move_l=wsin(m_SSP_Time_End_L,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_L,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift);
+ c_move_l=wsin(m_Time,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_R+PI,m_A_Move_Amplitude,m_A_Move_Amplitude_Shift);
+ x_move_r=wsin(m_Time,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_R+PI,-m_X_Move_Amplitude,-m_X_Move_Amplitude_Shift);
+ y_move_r=wsin(m_Time,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_R+PI,-m_Y_Move_Amplitude,-m_Y_Move_Amplitude_Shift);
+ z_move_r=wsin(m_Time,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_R,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift);
+ c_move_r=wsin(m_Time,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_R+PI,-m_A_Move_Amplitude,-m_A_Move_Amplitude_Shift);
+ pelvis_offset_l=wsin(m_Time,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_R,m_Pelvis_Offset/2,m_Pelvis_Offset/2);
+ pelvis_offset_r=wsin(m_Time,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_R,-m_Pelvis_Swing/2,-m_Pelvis_Swing/2);
+ }
+ else
+ {
+ x_move_l=wsin(m_SSP_Time_End_R,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_R+PI,m_X_Move_Amplitude,m_X_Move_Amplitude_Shift);
+ y_move_l=wsin(m_SSP_Time_End_R,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_R+PI,m_Y_Move_Amplitude,m_Y_Move_Amplitude_Shift);
+ z_move_l=wsin(m_SSP_Time_End_L,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_L,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift);
+ c_move_l=wsin(m_SSP_Time_End_R,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_R+PI,m_A_Move_Amplitude,m_A_Move_Amplitude_Shift);
+ x_move_r=wsin(m_SSP_Time_End_R,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_R+PI,-m_X_Move_Amplitude,-m_X_Move_Amplitude_Shift);
+ y_move_r=wsin(m_SSP_Time_End_R,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_R+PI,-m_Y_Move_Amplitude,-m_Y_Move_Amplitude_Shift);
+ z_move_r=wsin(m_SSP_Time_End_R,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_R,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift);
+ c_move_r=wsin(m_SSP_Time_End_R,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_R+PI,-m_A_Move_Amplitude,-m_A_Move_Amplitude_Shift);
+ pelvis_offset_l=0.0;
+ pelvis_offset_r=0.0;
+ }
+ a_move_l=0.0;
+ b_move_l=0.0;
+ a_move_r=0.0;
+ b_move_r=0.0;
+
+ ep[0]=x_swap+x_move_r+m_X_Offset;
+ ep[1]=y_swap+y_move_r-m_Y_Offset / 2.0;
+ ep[2]=z_swap+z_move_r+m_Z_Offset;
+ ep[3]=a_swap+a_move_r-m_R_Offset / 2.0;
+ ep[4]=b_swap+b_move_r+m_P_Offset;
+ ep[5]=c_swap+c_move_r-m_A_Offset / 2.0;
+ ep[6]=x_swap+x_move_l+m_X_Offset;
+ ep[7]=y_swap+y_move_l+m_Y_Offset / 2.0;
+ ep[8]=z_swap+z_move_l+m_Z_Offset;
+ ep[9]=a_swap+a_move_l+m_R_Offset / 2.0;
+ ep[10]=b_swap+b_move_l+m_P_Offset;
+ ep[11]=c_swap+c_move_l+m_A_Offset / 2.0;
+
+ // Compute body swing
+/* if(m_Time<=m_SSP_Time_End_L)
+ {
+ m_Body_Swing_Y=-ep[7];
+ m_Body_Swing_Z=ep[8];
+ }
+ else
+ {
+ m_Body_Swing_Y=-ep[1];
+ m_Body_Swing_Z=ep[2];
+ }
+ m_Body_Swing_Z-=DARWIN_LEG_LENGTH;*/
+
+ // Compute arm swing
+ if(m_X_Move_Amplitude==0)
+ {
+ angle[12]=0; // Right
+ angle[13]=0; // Left
+ }
+ else
+ {
+ angle[12]=wsin(m_Time,m_PeriodTime,0.0,-m_X_Move_Amplitude*m_Arm_Swing_Gain,0.0);
+ angle[13]=wsin(m_Time,m_PeriodTime,0.0,m_X_Move_Amplitude*m_Arm_Swing_Gain,0.0);
+ }
+
+ if(ram_data[DARWIN_WALK_CNTRL]&WALK_STATUS)
+ {
+ m_Time += walking_period;
+ if(m_Time >= m_PeriodTime)
+ m_Time = 0;
+ }
+
+ // Compute angles with the inverse kinematics
+ if((darwin_leg_ik(&angle[0],ep[0],ep[1],ep[2],ep[3],ep[4],ep[5])!=1) ||
+ (darwin_leg_ik(&angle[6],ep[6],ep[7],ep[8],ep[9],ep[10],ep[11])!=1))
+ return;// Do not use angles
+
+ // Compute motor value
+ if(manager_get_module(R_HIP_YAW)==MM_WALKING)
+ manager_current_angles[R_HIP_YAW]=((-180.0*angle[0])/PI)*65536.0;
+ if(manager_get_module(R_HIP_ROLL)==MM_WALKING)
+ manager_current_angles[R_HIP_ROLL]=((-180.0*(angle[1]+pelvis_offset_r))/PI)*65536.0;
+ if(manager_get_module(R_HIP_PITCH)==MM_WALKING)
+ manager_current_angles[R_HIP_PITCH]=((180.0*angle[2])/PI-m_Hip_Pitch_Offset)*65536.0;
+ if(manager_get_module(R_KNEE)==MM_WALKING)
+ manager_current_angles[R_KNEE]=((180.0*angle[3])/PI)*65536.0;
+ if(manager_get_module(R_ANKLE_PITCH)==MM_WALKING)
+ manager_current_angles[R_ANKLE_PITCH]=((-180.0*angle[4])/PI)*65536.0;
+ if(manager_get_module(R_ANKLE_ROLL)==MM_WALKING)
+ manager_current_angles[R_ANKLE_ROLL]=((180.0*angle[5])/PI)*65536.0;
+ if(manager_get_module(L_HIP_YAW)==MM_WALKING)
+ manager_current_angles[L_HIP_YAW]=((-180.0*angle[6])/PI)*65536.0;
+ if(manager_get_module(L_HIP_ROLL)==MM_WALKING)
+ manager_current_angles[L_HIP_ROLL]=((-180.0*(angle[7]+pelvis_offset_l))/PI)*65536.0;
+ if(manager_get_module(L_HIP_PITCH)==MM_WALKING)
+ manager_current_angles[L_HIP_PITCH]=((-180.0*angle[8])/PI+m_Hip_Pitch_Offset)*65536.0;
+ if(manager_get_module(L_KNEE)==MM_WALKING)
+ manager_current_angles[L_KNEE]=((-180.0*angle[9])/PI)*65536.0;
+ if(manager_get_module(L_ANKLE_PITCH)==MM_WALKING)
+ manager_current_angles[L_ANKLE_PITCH]=((180.0*angle[10])/PI)*65536.0;
+ if(manager_get_module(L_ANKLE_ROLL)==MM_WALKING)
+ manager_current_angles[L_ANKLE_ROLL]=((180.0*angle[11])/PI)*65536.0;
+ if(manager_get_module(R_SHOULDER_PITCH)==MM_WALKING)
+ manager_current_angles[R_SHOULDER_PITCH]=(angle[12]-48.345)*65536.0;
+ if(manager_get_module(L_SHOULDER_PITCH)==MM_WALKING)
+ manager_current_angles[L_SHOULDER_PITCH]=(-angle[13]+41.313)*65536.0;
+}
+
+// operation functions
+uint8_t walking_in_range(unsigned short int address, unsigned short int length)
+{
+ if(ram_in_window(WALK_BASE_ADDRESS,WALK_MEM_LENGTH,address,length) ||
+ ram_in_window(WALK_EEPROM_ADDRESS,WALK_EEPROM_LENGTH,address,length))
+ return 0x01;
+ else
+ return 0x00;
+}
+
+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)
+{
+ uint16_t i;
+
+ // walk control
+ if(ram_in_range(DARWIN_WALK_CNTRL,address,length))
+ {
+ if(data[DARWIN_WALK_CNTRL-address]&WALK_START)
+ walking_start();
+ if(data[DARWIN_WALK_CNTRL-address]&WALK_STOP)
+ walking_stop();
+ }
+ if(ram_in_range(DARWIN_WALK_STEP_FW_BW,address,length))
+ ram_data[DARWIN_WALK_STEP_FW_BW]=data[DARWIN_WALK_STEP_FW_BW-address];
+ if(ram_in_range(DARWIN_WALK_STEP_LEFT_RIGHT,address,length))
+ ram_data[DARWIN_WALK_STEP_LEFT_RIGHT]=data[DARWIN_WALK_STEP_LEFT_RIGHT-address];
+ if(ram_in_range(DARWIN_WALK_STEP_DIRECTION,address,length))
+ ram_data[DARWIN_WALK_STEP_DIRECTION]=data[DARWIN_WALK_STEP_DIRECTION-address];
+ // walk parameters
+ for(i=WALK_EEPROM_ADDRESS;i<=WALK_EEPROM_ADDRESS+WALK_EEPROM_LENGTH;i++)
+ if(ram_in_range(i,address,length))
+ ram_data[i]=data[i-address];
+}
+
--
GitLab