diff --git a/Makefile b/Makefile
index 19efbc3d6a6797b84b6d1b7bbca0e18875466d98..03acfec959fc69b72f0d2bcbd6385f1980b7cd59 100755
--- a/Makefile
+++ b/Makefile
@@ -86,7 +86,7 @@ $(BUILD_PATH)/%.o: $(USART_PATH)/src/%.c
$(CC) -c $(CFLAGS) -o $@ $<
$(MAIN_OUT_ELF): mkdir_build $(BIOLOID_OBJS) $(BUID_PATH)/$(STARTUP_FILE:.s=.o)
- $(LD) $(LDFLAGS) $(BIOLOID_OBJS) $(BUILD_PATH)/$(STARTUP_FILE:.s=.o) $(EXT_LIB) --output $@
+ $(LD) $(LDFLAGS) $(BIOLOID_OBJS) $(BUILD_PATH)/$(STARTUP_FILE:.s=.o) $(EXT_LIB) -lm --output $@
$(MAIN_OUT_HEX): $(MAIN_OUT_ELF)
$(OBJCP) $(OBJCPFLAGS_HEX) $< $@
diff --git a/include/action.h b/include/action.h
index 165841c6358b878366ad9b1515088b5425b8200f..adc182caebb376bab8247e6f3639932eee758919 100644
--- a/include/action.h
+++ b/include/action.h
@@ -8,9 +8,6 @@ extern "C" {
#include "stm32f4xx.h"
#include "motion_pages.h"
-extern int64_t action_current_angles[PAGE_MAX_NUM_SERVOS];
-extern uint8_t action_slope[PAGE_MAX_NUM_SERVOS];
-
// public functions
void action_init(uint16_t period_us);
inline void action_set_period(uint16_t period_us);
diff --git a/include/bioloid_kinematics.h b/include/bioloid_kinematics.h
new file mode 100755
index 0000000000000000000000000000000000000000..21662e4eafb58a278a58182ca1c6ea9853e18636
--- /dev/null
+++ b/include/bioloid_kinematics.h
@@ -0,0 +1,25 @@
+#ifndef _BIOLOID_KINEMATICS_H
+#define _BIOLOID_KINEMATICS_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "stm32f4xx.h"
+
+#define BIOLOID_LEG_SIDE_OFFSET 38.5 //mm
+#define BIOLOID_PELVIS_LENGTH 29.0 //mm
+#define BIOLOID_THIGH_LENGTH 76.388 //mm
+#define BIOLOID_CALF_LENGTH 76.388 //mm
+#define BIOLOID_KNEE_DEPTH 14.5 //mm
+#define BIOLOID_ANKLE_LENGTH 33.68 //mm
+#define BIOLOID_LEG_LENGTH (BIOLOID_THIGH_LENGTH + BIOLOID_CALF_LENGTH + BIOLOID_ANKLE_LENGTH) //mm
+
+// public functions
+uint8_t bioloid_leg_ik(float *out, float x, float y, float z, float a, float b, float c);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/include/bioloid_math.h b/include/bioloid_math.h
new file mode 100755
index 0000000000000000000000000000000000000000..fb47cbd74b167d1833825a729c05724fedf092aa
--- /dev/null
+++ b/include/bioloid_math.h
@@ -0,0 +1,62 @@
+#ifndef _BIOLOID_MATH_H
+#define _BIOLOID_MATH_H
+
+#include "stm32f4xx.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);
+
+#endif
diff --git a/include/bioloid_registers.h b/include/bioloid_registers.h
index 54f7994babffa26bd1b10409f811af1846fbc203..3a18e31db477ea33f2527f5f7964e25df0ba6a27 100644
--- a/include/bioloid_registers.h
+++ b/include/bioloid_registers.h
@@ -50,6 +50,23 @@ extern "C" {
#define MM_SERVO31_OFFSET ((unsigned short int)0x0030)
#define GYRO_FB_ADC_CH_OFFSET ((unsigned short int)0x0031)
#define GYRO_LR_ADC_CH_OFFSET ((unsigned short int)0x0032)
+#define WALK_X_OFFSET ((unsigned short int)0x0033)
+#define WALK_Y_OFFSET ((unsigned short int)0x0034)
+#define WALK_Z_OFFSET ((unsigned short int)0x0035)
+#define WALK_ROLL_OFFSET ((unsigned short int)0x0036)
+#define WALK_PITCH_OFFSET ((unsigned short int)0x0037)
+#define WALK_YAW_OFFSET ((unsigned short int)0x0038)
+#define WALK_HIP_PITCH_OFF ((unsigned short int)0x0039)
+#define WALK_PERIOD_TIME ((unsigned short int)0x003B)
+#define WALK_DSP_RATIO ((unsigned short int)0x003D)
+#define WALK_STEP_FW_BW_RATIO ((unsigned short int)0x003E)
+#define WALK_FOOT_HEIGHT ((unsigned short int)0x003F)
+#define WALK_SWING_RIGHT_LEFT ((unsigned short int)0x0040)
+#define WALK_SWING_TOP_DOWN ((unsigned short int)0x0041)
+#define WALK_PELVIS_OFFSET ((unsigned short int)0x0042)
+#define WALK_ARM_SWING_GAIN ((unsigned short int)0x0043)
+#define WALK_MAX_VEL ((unsigned short int)0x0044)
+#define WALK_MAX_ROT_VEL ((unsigned short int)0x0045)
#define LAST_EEPROM_OFFSET ((unsigned short int)0x00FF)
@@ -105,6 +122,25 @@ typedef enum {
BIOLOID_MM_SERVO31_OFFSET = MM_SERVO31_OFFSET, // angle offset in fixed point format 1-3|4
BIOLIOD_GYRO_FB_ADC_CH = GYRO_FB_ADC_CH_OFFSET,
BIOLIOD_GYRO_LR_ADC_CH = GYRO_LR_ADC_CH_OFFSET,
+ BIOLOID_WALK_X_OFFSET = WALK_X_OFFSET,
+ BIOLOID_WALK_Y_OFFSET = WALK_Y_OFFSET,
+ BIOLOID_WALK_Z_OFFSET = WALK_Z_OFFSET,
+ BIOLOID_WALK_ROLL_OFFSET = WALK_ROLL_OFFSET,
+ BIOLOID_WALK_PITCH_OFFSET = WALK_PITCH_OFFSET,
+ BIOLOID_WALK_YAW_OFFSET = WALK_YAW_OFFSET,
+ BIOLOID_WALK_HIP_PITCH_OFF_L = WALK_HIP_PITCH_OFF,
+ BIOLOID_WALK_HIP_PITCH_OFF_H = WALK_HIP_PITCH_OFF+1,
+ BIOLOID_WALK_PERIOD_TIME_L = WALK_PERIOD_TIME,
+ BIOLOID_WALK_PERIOD_TIME_H = WALK_PERIOD_TIME+1,
+ BIOLOID_WALK_DSP_RATIO = WALK_DSP_RATIO,
+ BIOLOID_WALK_STEP_FW_BW_RATIO = WALK_STEP_FW_BW_RATIO,
+ BIOLOID_WALK_FOOT_HEIGHT = WALK_FOOT_HEIGHT,
+ BIOLOID_WALK_SWING_RIGHT_LEFT = WALK_SWING_RIGHT_LEFT,
+ BIOLOID_WALK_SWING_TOP_DOWN = WALK_SWING_TOP_DOWN,
+ BIOLOID_WALK_PELVIS_OFFSET = WALK_PELVIS_OFFSET,
+ BIOLOID_WALK_ARM_SWING_GAIN = WALK_ARM_SWING_GAIN,
+ BIOLOID_WALK_MAX_VEL = WALK_MAX_VEL,
+ BIOLOID_WALK_MAX_ROT_VEL = WALK_MAX_ROT_VEL,
BIOLOID_USER1_LED_CNTRL = 0x0100, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
// | | | | blink | toggle | value | internally used
BIOLOID_USER1_LED_PERIOD_L = 0x0101,
@@ -268,7 +304,12 @@ typedef enum {
BIOLOID_GYRO_FWD_FALL_THD = 0x017E,
BIOLOID_GYRO_BWD_FALL_THD = 0x017F,
BIOLOID_GYRO_LEFT_FALL_THD = 0x0180,
- BOILOID_GYRO_RIGHT_FALL_THD = 0x0181
+ BOILOID_GYRO_RIGHT_FALL_THD = 0x0181,
+ BIOLOID_WALK_CNTRL = 0x0182, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // current phase walking stop walking start walking
+ BIOLOID_WALK_STEP_FW_BW = 0x0183,
+ BIOLOID_WALK_STEP_LEFT_RIGHT = 0x0184,
+ BIOLOID_WALK_STEP_DIRECTION = 0x0185
} bioloid_registers;
#define GPIO_BASE_ADDRESS 0x0100
@@ -291,7 +332,10 @@ typedef enum {
#define MANAGER_BASE_ADDRESS 0x0128
#define MANAGER_MEM_LENGTH 82
-#define MANAGER_EEPROM_LENGTH 10
+#define MANAGER_EEPROM_BASE1 0x0006
+#define MANAGER_EEPROM_LENGTH1 10
+#define MANAGER_EEPROM_BASE2 0x0011
+#define MANAGER_EEPROM_LENGTH2 32
#define MANAGER_ENABLE 0x01
#define MANAGER_EN_BAL 0x02
#define MANAGER_EN_PWR 0x04
@@ -315,6 +359,15 @@ typedef enum {
#define GYRO_EN_FALL_DET 0x04
#define GYRO_EN_FALL_DET_INT 0x08
+#define WALK_BASE_ADDRESS 0x0182
+#define WALK_MEM_LENGTH 4
+#define WALK_EEPROM_ADDRESS 0x0033
+#define WALK_EEPROM_LENGTH 19
+#define WALK_START 0x01
+#define WALK_STOP 0x02
+#define WALK_STATUS 0x10
+#define WALK_PHASE 0xC0
+
#ifdef __cplusplus
}
#endif
diff --git a/include/eeprom.h b/include/eeprom.h
index bb184e86d8847e8f63129b9896b9163d7d99f8e0..68a136a6dd06dfe2cf74de8f0bda06b465338a17 100755
--- a/include/eeprom.h
+++ b/include/eeprom.h
@@ -83,7 +83,7 @@ extern "C" {
#define PAGE_FULL ((uint8_t)0x80)
/* Variables' number */
-#define NB_OF_VAR ((uint8_t)0x33)
+#define NB_OF_VAR ((uint8_t)0x46)
/* Exported types ------------------------------------------------------------*/
/* Exported macro ------------------------------------------------------------*/
diff --git a/include/walking.h b/include/walking.h
new file mode 100755
index 0000000000000000000000000000000000000000..5beb8ce50e665d91d4d42af8f5a32850acdee6ac
--- /dev/null
+++ b/include/walking.h
@@ -0,0 +1,30 @@
+#ifndef _WALKING_H
+#define _WALKING_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "stm32f4xx.h"
+#include "motion_manager.h"
+
+// public functions
+void walking_init(uint16_t period_us);
+inline void walking_set_period(uint16_t period_us);
+inline 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/src/bioloid_dyn_slave.c b/src/bioloid_dyn_slave.c
index b2a0edfd9a9a3adb67f6326be462ce8eff90c42b..f58fbcf22696380e21ba63c4b288b13c95ce4e0d 100644
--- a/src/bioloid_dyn_slave.c
+++ b/src/bioloid_dyn_slave.c
@@ -53,23 +53,23 @@ unsigned char bioloid_on_write(unsigned short int address,unsigned short int len
if(ram_in_range(BIOLOID_ID,address,length))
{
dyn_slave_set_address(&bioloid_dyn_slave,data[BIOLOID_ID-address]);
- ram_write_byte(BIOLOID_ID,data[BIOLOID_ID-address]);
+ ram_data[BIOLOID_ID]=data[BIOLOID_ID-address];
}
if(ram_in_range(BIOLOID_BAUD_RATE,address,length))
{
bioloid_comm_init.BaudRate=2000000/(data[BIOLOID_BAUD_RATE-address]+1);
usart3_config(&bioloid_dyn_slave_comm,&bioloid_comm_init);
- ram_write_byte(BIOLOID_BAUD_RATE,data[BIOLOID_BAUD_RATE-address]);
+ ram_data[BIOLOID_BAUD_RATE]=data[BIOLOID_BAUD_RATE-address];
}
if(ram_in_range(BIOLOID_RETURN_DELAY_TIME,address,length))
{
dyn_slave_set_return_delay(&bioloid_dyn_slave,data[BIOLOID_RETURN_DELAY_TIME-address]);
- ram_write_byte(BIOLOID_RETURN_DELAY_TIME,data[BIOLOID_RETURN_DELAY_TIME-address]);
+ ram_data[BIOLOID_RETURN_DELAY_TIME]=data[BIOLOID_RETURN_DELAY_TIME-address];
}
if(ram_in_range(BIOLOID_RETURN_LEVEL,address,length))
{
dyn_slave_set_return_level(&bioloid_dyn_slave,data[BIOLOID_RETURN_LEVEL-address]);
- ram_write_byte(BIOLOID_RETURN_LEVEL,data[BIOLOID_RETURN_LEVEL-address]);
+ ram_data[BIOLOID_RETURN_LEVEL]=data[BIOLOID_RETURN_LEVEL-address];
}
// GPIO commands
if(gpio_in_range(address,length))
@@ -98,7 +98,6 @@ unsigned char bioloid_on_write(unsigned short int address,unsigned short int len
void bioloid_on_ping(void)
{
- gpio_toggle_led(RXD_LED);
}
/* interrupt service routines */
diff --git a/src/bioloid_gyro.c b/src/bioloid_gyro.c
index 7c4d3dab8f5edfd6e95365d192e6fd4877bf81e0..a1fec04b0cca6c24956c16aca6ad065bd3608600 100644
--- a/src/bioloid_gyro.c
+++ b/src/bioloid_gyro.c
@@ -72,6 +72,10 @@ void gyro_process_read_cmd(unsigned short int address,unsigned short int length,
void gyro_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
{
-
+ // gyro channels
+ if(ram_in_range(BIOLIOD_GYRO_FB_ADC_CH,address,length))
+ ram_data[BIOLIOD_GYRO_FB_ADC_CH]=data[BIOLIOD_GYRO_FB_ADC_CH-address];
+ if(ram_in_range(BIOLIOD_GYRO_LR_ADC_CH,address,length))
+ ram_data[BIOLIOD_GYRO_LR_ADC_CH]=data[BIOLIOD_GYRO_LR_ADC_CH-address];
}
diff --git a/src/bioloid_kinematics.c b/src/bioloid_kinematics.c
new file mode 100755
index 0000000000000000000000000000000000000000..60a278934f42df8d4115a5d7706a4757486da541
--- /dev/null
+++ b/src/bioloid_kinematics.c
@@ -0,0 +1,82 @@
+#include "bioloid_kinematics.h"
+#include "bioloid_math.h"
+#include <math.h>
+
+uint8_t bioloid_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 - BIOLOID_LEG_LENGTH);
+ matrix3d_set_transform(&Tad,&position,&orientation);
+
+ vec.x = x + Tad.coef[2] * BIOLOID_ANKLE_LENGTH;
+ vec.y = y + Tad.coef[6] * BIOLOID_ANKLE_LENGTH;
+ vec.z = (z - BIOLOID_LEG_LENGTH) + Tad.coef[10] * BIOLOID_ANKLE_LENGTH;
+
+ // Get Knee
+ _Rac = vector3d_length(&vec);
+ _Acos = acos((_Rac * _Rac - BIOLOID_THIGH_LENGTH * BIOLOID_THIGH_LENGTH - BIOLOID_CALF_LENGTH * BIOLOID_CALF_LENGTH) / (2 * BIOLOID_THIGH_LENGTH * BIOLOID_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] - BIOLOID_ANKLE_LENGTH) * (Tda.coef[11] - BIOLOID_ANKLE_LENGTH));
+ _m = (_k * _k - _l * _l - BIOLOID_ANKLE_LENGTH * BIOLOID_ANKLE_LENGTH) / (2 * _l * BIOLOID_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, -BIOLOID_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)) * BIOLOID_CALF_LENGTH;
+ _l = -BIOLOID_THIGH_LENGTH - cos(*(out + 3)) * BIOLOID_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/src/bioloid_math.c b/src/bioloid_math.c
new file mode 100755
index 0000000000000000000000000000000000000000..3f42bbdbb7b022ddf7933c847e030d927cf2d4dc
--- /dev/null
+++ b/src/bioloid_math.c
@@ -0,0 +1,237 @@
+#include "bioloid_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 * PI / 180.0);
+ float Cy = cos(vector->y * PI / 180.0);
+ float Cz = cos(vector->z * PI / 180.0);
+ float Sx = sin(vector->x * PI / 180.0);
+ float Sy = sin(vector->y * PI / 180.0);
+ float Sz = sin(vector->z * PI / 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/src/eeprom.c b/src/eeprom.c
index 5d965d6f0befaadad4f5aee206c194d73f8e8aa4..48aa5e5c3f1ae341c3775a95db66160ffba1f491 100755
--- a/src/eeprom.c
+++ b/src/eeprom.c
@@ -46,7 +46,6 @@
/* Private typedef -----------------------------------------------------------*/
/* Private define ------------------------------------------------------------*/
-#define EEPROM_SIZE 0x20
#define DEFAULT_DEVICE_MODEL 0x7300
#define DEFAULT_FIRMWARE_VERSION 0x0001
#define DEFAULT_DEVICE_ID 0x0002
@@ -60,38 +59,56 @@
#define DEFAULT_GYRO_FB_ADC_CH (uint16_t)ADC_CH1
#define DEFAULT_GYRO_LR_ADC_CH (uint16_t)ADC_CH2
#define DEFAULT_RETURN_LEVEL 0x0002
-#define DEFAULT_SERVO0_OFFSET 0x0000
-#define DEFAULT_SERVO1_OFFSET 0x0000
-#define DEFAULT_SERVO2_OFFSET 0x0000
-#define DEFAULT_SERVO3_OFFSET 0x0000
-#define DEFAULT_SERVO4_OFFSET 0x0000
-#define DEFAULT_SERVO5_OFFSET 0x0000
-#define DEFAULT_SERVO6_OFFSET 0x0000
-#define DEFAULT_SERVO7_OFFSET 0x0000
-#define DEFAULT_SERVO8_OFFSET 0x0000
-#define DEFAULT_SERVO9_OFFSET 0x0000
-#define DEFAULT_SERVO10_OFFSET 0x0000
-#define DEFAULT_SERVO11_OFFSET 0x0000
-#define DEFAULT_SERVO12_OFFSET 0x0000
-#define DEFAULT_SERVO13_OFFSET 0x0000
-#define DEFAULT_SERVO14_OFFSET 0x0000
-#define DEFAULT_SERVO15_OFFSET 0x0000
-#define DEFAULT_SERVO16_OFFSET 0x0000
-#define DEFAULT_SERVO17_OFFSET 0x0000
-#define DEFAULT_SERVO18_OFFSET 0x0000
-#define DEFAULT_SERVO19_OFFSET 0x0000
-#define DEFAULT_SERVO20_OFFSET 0x0000
-#define DEFAULT_SERVO21_OFFSET 0x0000
-#define DEFAULT_SERVO22_OFFSET 0x0000
-#define DEFAULT_SERVO23_OFFSET 0x0000
-#define DEFAULT_SERVO24_OFFSET 0x0000
-#define DEFAULT_SERVO25_OFFSET 0x0000
-#define DEFAULT_SERVO26_OFFSET 0x0000
-#define DEFAULT_SERVO27_OFFSET 0x0000
-#define DEFAULT_SERVO28_OFFSET 0x0000
-#define DEFAULT_SERVO29_OFFSET 0x0000
-#define DEFAULT_SERVO30_OFFSET 0x0000
-#define DEFAULT_SERVO31_OFFSET 0x0000
+#define DEFAULT_SERVO0_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO1_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO2_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO3_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO4_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO5_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO6_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO7_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO8_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO9_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO10_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO11_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO12_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO13_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO14_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO15_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO16_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO17_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO18_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO19_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO20_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO21_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO22_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO23_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO24_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO25_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO26_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO27_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO28_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO29_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO30_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO31_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_WALK_X_OFFSET 0xFFF6 // -10 mm
+#define DEFAULT_WALK_Y_OFFSET 0x0005 // 5 mm
+#define DEFAULT_WALK_Z_OFFSET 0x0014 // 20 mm
+#define DEFAULT_WALK_ROLL_OFFSET 0x0000 // 0 degrees in fixed point format 5 (1+4) | 3
+#define DEFAULT_WALK_PITCH_OFFSET 0x0000 // 0 degrees in fixed point format 5 (1+4) | 3
+#define DEFAULT_WALK_YAW_OFFSET 0x0000 // 0 degrees in fixed point format 5 (1+4) | 3
+#define DEFAULT_WALK_HIP_PITCH_OFF 0x0000 // 0 degrees in fixed point format 6 (1+5) | 10
+#define DEFAULT_WALK_PERIOD_TIME 0x0258 // 600 ms
+#define DEFAULT_WALK_DSP_RATIO 0x0019 // 0.1 in fixed point format 0 | 8
+#define DEFAULT_WALK_STEP_FW_BW_RATIO 0x004C // 0.3 in fixed point format 0 | 8
+#define DEFAULT_WALK_FOOT_HEIGHT 0x0028 // 40 mm
+#define DEFAULT_WALK_SWING_RIGHT_LEFT 0x0014 // 20 mm
+#define DEFAULT_WALK_SWING_TOP_DOWN 0x0005 // 5 mm
+#define DEFAULT_WALK_PELVIS_OFFSET 0x0018 // 3 degrees in fixed point format 5 (1+4) | 3
+#define DEFAULT_WALK_ARM_SWING_GAIN 0x0030 // 1.5 in fixed point format 3 | 5
+#define DEFAULT_WALK_MAX_VEL 0x0016 // 20 mm/s
+#define DEFAULT_WALK_MAX_ROT_VEL 0x0008 // 8 degrees/s in fixed point format 5 | 3
+
/* Private macro -------------------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
@@ -150,7 +167,26 @@ uint16_t eeprom_data[] __attribute__ ((section (".eeprom")))={VALID_PAGE,
DEFAULT_SERVO30_OFFSET, MM_SERVO30_OFFSET,
DEFAULT_SERVO31_OFFSET, MM_SERVO31_OFFSET,
DEFAULT_GYRO_FB_ADC_CH, GYRO_FB_ADC_CH_OFFSET,
- DEFAULT_GYRO_LR_ADC_CH, GYRO_LR_ADC_CH_OFFSET};
+ DEFAULT_GYRO_LR_ADC_CH, GYRO_LR_ADC_CH_OFFSET,
+ DEFAULT_WALK_X_OFFSET, WALK_X_OFFSET,
+ DEFAULT_WALK_Y_OFFSET, WALK_Y_OFFSET,
+ DEFAULT_WALK_Z_OFFSET, WALK_Z_OFFSET,
+ DEFAULT_WALK_ROLL_OFFSET, WALK_ROLL_OFFSET,
+ DEFAULT_WALK_PITCH_OFFSET, WALK_PITCH_OFFSET,
+ DEFAULT_WALK_YAW_OFFSET, WALK_YAW_OFFSET,
+ DEFAULT_WALK_HIP_PITCH_OFF&0xFF, WALK_HIP_PITCH_OFF,
+ DEFAULT_WALK_HIP_PITCH_OFF>>8, WALK_HIP_PITCH_OFF+1,
+ DEFAULT_WALK_PERIOD_TIME&0xFF, WALK_PERIOD_TIME,
+ DEFAULT_WALK_PERIOD_TIME>>8, WALK_PERIOD_TIME+1,
+ DEFAULT_WALK_DSP_RATIO, WALK_DSP_RATIO,
+ DEFAULT_WALK_STEP_FW_BW_RATIO, WALK_STEP_FW_BW_RATIO,
+ DEFAULT_WALK_FOOT_HEIGHT, WALK_FOOT_HEIGHT,
+ DEFAULT_WALK_SWING_RIGHT_LEFT, WALK_SWING_RIGHT_LEFT,
+ DEFAULT_WALK_SWING_TOP_DOWN, WALK_SWING_TOP_DOWN,
+ DEFAULT_WALK_PELVIS_OFFSET, WALK_PELVIS_OFFSET,
+ DEFAULT_WALK_ARM_SWING_GAIN, WALK_ARM_SWING_GAIN,
+ DEFAULT_WALK_MAX_VEL, WALK_MAX_VEL,
+ DEFAULT_WALK_MAX_ROT_VEL, WALK_MAX_ROT_VEL};
/* Private function prototypes -----------------------------------------------*/
/* Private functions ---------------------------------------------------------*/
diff --git a/src/motion_manager.c b/src/motion_manager.c
index 97b7cf0da77fdb593f1c9fc11a82f5adc83622fc..ba2e387a06798ce70b4e8c05a9c316e304ca6a5b 100644
--- a/src/motion_manager.c
+++ b/src/motion_manager.c
@@ -5,7 +5,7 @@
#include "action.h"
#include "adc_dma.h"
#include "bioloid_gyro.h"
-#include "gpio.h"
+#include "walking.h"
#define MANAGER_TIMER TIM3
#define ENABLE_MANAGER_TIMER_CLK __HAL_RCC_TIM3_CLK_ENABLE()
@@ -99,14 +99,11 @@ void manager_get_target_position(void)
{
if(manager_servos[i].enabled && manager_servos[i].model!=0x0000)// servo is enabled and present
{
- if(manager_servos[i].module==MM_ACTION)
- {
- manager_servos[i].current_angle=((manager_current_angles[i]>>9)+manager_balance_offset[i]+manager_offset[i]);
- //>>16 from the action codification, <<7 from the manager codification
- manager_servos[i].current_value=manager_angle_to_value(i,manager_servos[i].current_angle);
- ram_data[BIOLOID_MM_SERVO0_CUR_POS_L+i*2]=(manager_servos[i].current_angle&0xFF);
- ram_data[BIOLOID_MM_SERVO0_CUR_POS_H+i*2]=(manager_servos[i].current_angle>>8);
- }
+ manager_servos[i].current_angle=((manager_current_angles[i]>>9)+manager_balance_offset[i]+manager_offset[i]);
+ //>>16 from the action codification, <<7 from the manager codification
+ manager_servos[i].current_value=manager_angle_to_value(i,manager_servos[i].current_angle);
+ ram_data[BIOLOID_MM_SERVO0_CUR_POS_L+i*2]=(manager_servos[i].current_angle&0xFF);
+ ram_data[BIOLOID_MM_SERVO0_CUR_POS_H+i*2]=(manager_servos[i].current_angle>>8);
}
}
}
@@ -161,7 +158,7 @@ void MANAGER_TIMER_IRQHandler(void)
// call the joint motion process
// joint_motion_process();
// call the walking process
- // walking_process();
+ walking_process();
// balance the robot
manager_balance();
// get the target angles from all modules
@@ -329,10 +326,10 @@ void manager_init(uint16_t period_us)
manager_set_offset(i,(signed char)ram_data[BIOLOID_MM_SERVO0_OFFSET+i]);
}
manager_balance_enabled=0x00;
- gpio_set_led(TXD_LED);
/* initialize action module */
action_init(period_us);
+ walking_init(period_us);
}
uint16_t manager_get_period(void)
@@ -351,6 +348,8 @@ void manager_set_period(uint16_t period_us)
manager_motion_period_us=period_us;
ram_data[BIOLOID_MM_PERIOD_L]=period_us&0x00FF;
ram_data[BIOLOID_MM_PERIOD_H]=(period_us&0xFF00)>>8;
+ action_set_period(period_us);
+ walking_set_period(period_us);
}
inline void manager_enable(void)
@@ -483,8 +482,8 @@ inline int16_t manager_get_ccw_angle_limit(uint8_t servo_id)
uint8_t manager_in_range(unsigned short int address, unsigned short int length)
{
if(ram_in_window(MANAGER_BASE_ADDRESS,MANAGER_MEM_LENGTH,address,length) ||
- ram_in_window(BIOLOID_MM_PERIOD_L,10,address,length) ||
- ram_in_window(BIOLOID_MM_SERVO0_OFFSET,MANAGER_MAX_NUM_SERVOS,address,length))
+ ram_in_window(MANAGER_EEPROM_BASE1,MANAGER_EEPROM_LENGTH1,address,length) ||
+ ram_in_window(MANAGER_EEPROM_BASE2,MANAGER_EEPROM_LENGTH2,address,length))
return 0x01;
else
return 0x00;
@@ -535,4 +534,8 @@ void manager_process_write_cmd(unsigned short int address,unsigned short int len
else
bioloid_dyn_master_servos_disable_power();
}
+ // balance gains
+ for(i=MM_BAL_KNEE_GAIN_OFFSET;i<=MM_BAL_HIP_ROLL_GAIN_OFFSET+1;i++)
+ if(ram_in_range(i,address,length))
+ ram_data[i]=data[i-address];
}
diff --git a/src/ram.c b/src/ram.c
index 45069ef7f06a94cc647195685d5b76ea480a55bf..34142288c21d200e51380b6f2ece9eb61c3e1f03 100644
--- a/src/ram.c
+++ b/src/ram.c
@@ -53,6 +53,44 @@ void ram_init(void)
ram_data[GYRO_FB_ADC_CH_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
if(EE_ReadVariable(GYRO_LR_ADC_CH_OFFSET,&eeprom_data)==0)
ram_data[GYRO_LR_ADC_CH_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
+ if(EE_ReadVariable(WALK_X_OFFSET,&eeprom_data)==0)
+ ram_data[WALK_X_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
+ if(EE_ReadVariable(WALK_Y_OFFSET,&eeprom_data)==0)
+ ram_data[WALK_Y_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
+ if(EE_ReadVariable(WALK_Z_OFFSET,&eeprom_data)==0)
+ ram_data[WALK_Z_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
+ if(EE_ReadVariable(WALK_ROLL_OFFSET,&eeprom_data)==0)
+ ram_data[WALK_ROLL_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
+ if(EE_ReadVariable(WALK_PITCH_OFFSET,&eeprom_data)==0)
+ ram_data[WALK_PITCH_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
+ if(EE_ReadVariable(WALK_YAW_OFFSET,&eeprom_data)==0)
+ ram_data[WALK_YAW_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
+ if(EE_ReadVariable(WALK_HIP_PITCH_OFF,&eeprom_data)==0)
+ ram_data[WALK_HIP_PITCH_OFF]=(uint8_t)(eeprom_data&0x00FF);
+ if(EE_ReadVariable(WALK_HIP_PITCH_OFF+1,&eeprom_data)==0)
+ ram_data[WALK_HIP_PITCH_OFF+1]=(uint8_t)(eeprom_data&0x00FF);
+ if(EE_ReadVariable(WALK_PERIOD_TIME,&eeprom_data)==0)
+ ram_data[WALK_PERIOD_TIME]=(uint8_t)(eeprom_data&0x00FF);
+ if(EE_ReadVariable(WALK_PERIOD_TIME+1,&eeprom_data)==0)
+ ram_data[WALK_PERIOD_TIME+1]=(uint8_t)(eeprom_data&0x00FF);
+ if(EE_ReadVariable(WALK_DSP_RATIO,&eeprom_data)==0)
+ ram_data[WALK_DSP_RATIO]=(uint8_t)(eeprom_data&0x00FF);
+ if(EE_ReadVariable(WALK_STEP_FW_BW_RATIO,&eeprom_data)==0)
+ ram_data[WALK_STEP_FW_BW_RATIO]=(uint8_t)(eeprom_data&0x00FF);
+ if(EE_ReadVariable(WALK_FOOT_HEIGHT,&eeprom_data)==0)
+ ram_data[WALK_FOOT_HEIGHT]=(uint8_t)(eeprom_data&0x00FF);
+ if(EE_ReadVariable(WALK_SWING_RIGHT_LEFT,&eeprom_data)==0)
+ ram_data[WALK_SWING_RIGHT_LEFT]=(uint8_t)(eeprom_data&0x00FF);
+ if(EE_ReadVariable(WALK_SWING_TOP_DOWN,&eeprom_data)==0)
+ ram_data[WALK_SWING_TOP_DOWN]=(uint8_t)(eeprom_data&0x00FF);
+ if(EE_ReadVariable(WALK_PELVIS_OFFSET,&eeprom_data)==0)
+ ram_data[WALK_PELVIS_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
+ if(EE_ReadVariable(WALK_ARM_SWING_GAIN,&eeprom_data)==0)
+ ram_data[WALK_ARM_SWING_GAIN]=(uint8_t)(eeprom_data&0x00FF);
+ if(EE_ReadVariable(WALK_MAX_VEL,&eeprom_data)==0)
+ ram_data[WALK_MAX_VEL]=(uint8_t)(eeprom_data&0x00FF);
+ if(EE_ReadVariable(WALK_MAX_ROT_VEL,&eeprom_data)==0)
+ ram_data[WALK_MAX_ROT_VEL]=(uint8_t)(eeprom_data&0x00FF);
}
inline void ram_read_byte(uint16_t address,uint8_t *data)
diff --git a/src/walking.c b/src/walking.c
new file mode 100755
index 0000000000000000000000000000000000000000..86f808d9813a098abfe2a2c252f40892d4faaed1
--- /dev/null
+++ b/src/walking.c
@@ -0,0 +1,506 @@
+#include "walking.h"
+#include "bioloid_kinematics.h"
+#include "bioloid_math.h"
+#include "ram.h"
+#include <math.h>
+
+enum {PHASE0=0x00,PHASE1=0x40,PHASE2=0x80,PHASE3=0xC0};
+
+// Walking control
+float X_MOVE_AMPLITUDE=0.0;//mm
+float Y_MOVE_AMPLITUDE=0.0;//mm
+float A_MOVE_AMPLITUDE=0.0;//degrees
+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
+inline 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=ram_data[BIOLOID_WALK_PERIOD_TIME_L]+ram_data[BIOLOID_WALK_PERIOD_TIME_H];
+ m_SSP_Ratio=1.0-((float)ram_data[BIOLOID_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[BIOLOID_WALK_PELVIS_OFFSET])/8.0;
+ m_Pelvis_Swing=m_Pelvis_Offset*0.35;
+ m_Arm_Swing_Gain=((float)ram_data[BIOLOID_WALK_ARM_SWING_GAIN])/32.0;
+}
+
+void update_param_move()
+{
+ static float current_x_amplitude=0;
+ static float current_y_amplitude=0;
+ static float current_a_amplitude=0;
+
+ // change longitudinal and transversal velocities to get to the target ones
+ if(current_x_amplitude<X_MOVE_AMPLITUDE)
+ {
+ current_x_amplitude+=(((float)ram_data[BIOLOID_WALK_MAX_VEL])*m_PeriodTime)/1000.0;
+ if(current_x_amplitude>X_MOVE_AMPLITUDE)
+ current_x_amplitude=X_MOVE_AMPLITUDE;
+ }
+ else if(current_x_amplitude>X_MOVE_AMPLITUDE)
+ {
+ current_x_amplitude-=(((float)ram_data[BIOLOID_WALK_MAX_VEL])*m_PeriodTime)/1000.0;
+ if(current_x_amplitude<X_MOVE_AMPLITUDE)
+ current_x_amplitude=X_MOVE_AMPLITUDE;
+ }
+ m_X_Move_Amplitude=current_x_amplitude;
+ m_X_Swap_Amplitude=current_x_amplitude*((float)ram_data[BIOLOID_WALK_STEP_FW_BW_RATIO])/256.0;
+
+ // Right/Left
+ if(current_y_amplitude<Y_MOVE_AMPLITUDE)
+ {
+ current_y_amplitude+=(((float)ram_data[BIOLOID_WALK_MAX_VEL])*m_PeriodTime)/1000.0;
+ if(current_y_amplitude>Y_MOVE_AMPLITUDE)
+ current_y_amplitude=Y_MOVE_AMPLITUDE;
+ }
+ else if(current_y_amplitude>Y_MOVE_AMPLITUDE)
+ {
+ current_y_amplitude-=(((float)ram_data[BIOLOID_WALK_MAX_VEL])*m_PeriodTime)/1000.0;
+ if(current_y_amplitude<Y_MOVE_AMPLITUDE)
+ current_y_amplitude=Y_MOVE_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[BIOLOID_WALK_SWING_RIGHT_LEFT])+m_Y_Move_Amplitude_Shift*0.04;
+
+ m_Z_Move_Amplitude=((float)ram_data[BIOLOID_WALK_FOOT_HEIGHT])/2.0;
+ m_Z_Move_Amplitude_Shift=m_Z_Move_Amplitude / 2.0;
+ m_Z_Swap_Amplitude=((float)ram_data[BIOLOID_WALK_SWING_TOP_DOWN]);
+ m_Z_Swap_Amplitude_Shift=m_Z_Swap_Amplitude;
+
+ // Direction
+ if(current_a_amplitude<A_MOVE_AMPLITUDE)
+ {
+ current_a_amplitude+=(((float)ram_data[BIOLOID_WALK_MAX_ROT_VEL])*m_PeriodTime)/8000.0;
+ if(current_a_amplitude>A_MOVE_AMPLITUDE)
+ current_a_amplitude=A_MOVE_AMPLITUDE;
+ }
+ else if(current_a_amplitude>A_MOVE_AMPLITUDE)
+ {
+ current_a_amplitude-=(((float)ram_data[BIOLOID_WALK_MAX_ROT_VEL])*m_PeriodTime)/8000.0;
+ if(current_a_amplitude<A_MOVE_AMPLITUDE)
+ current_a_amplitude=A_MOVE_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[BIOLOID_WALK_X_OFFSET]));
+ m_Y_Offset = ((float)((int8_t)ram_data[BIOLOID_WALK_Y_OFFSET]));
+ m_Z_Offset = ((float)((int8_t)ram_data[BIOLOID_WALK_Z_OFFSET]));
+ m_R_Offset = ((float)((int8_t)ram_data[BIOLOID_WALK_ROLL_OFFSET]))*PI/1440.0;// (r_offset/8)*(pi/180)
+ m_P_Offset = ((float)((int8_t)ram_data[BIOLOID_WALK_PITCH_OFFSET]))*PI/1440.0;// (p_offset/8)*(pi/180)
+ m_A_Offset = ((float)((int8_t)ram_data[BIOLOID_WALK_YAW_OFFSET]))*PI/1440.0;// (a_offset/8)*(pi/180)
+ m_Hip_Pitch_Offset = ((float)((int16_t)(ram_data[BIOLOID_WALK_HIP_PITCH_OFF_L]+(ram_data[BIOLOID_WALK_HIP_PITCH_OFF_L]<<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();
+}
+
+inline void walking_set_period(uint16_t period_us)
+{
+ walking_period=((float)period_us)/1000.0;
+}
+
+inline uint16_t walking_get_period(void)
+{
+ return (uint16_t)(walking_period*1000);
+}
+
+void walking_start(void)
+{
+ ram_data[BIOLOID_WALK_CNTRL]|=WALK_STATUS;
+ m_Ctrl_Running=0x01;
+}
+
+void walking_stop(void)
+{
+ m_Ctrl_Running=0x00;
+}
+
+uint8_t is_walking(void)
+{
+ if(ram_data[BIOLOID_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[BIOLOID_WALK_CNTRL]&=(~WALK_PHASE);
+ ram_data[BIOLOID_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[BIOLOID_WALK_CNTRL]&=(~WALK_STATUS);
+ else
+ {
+ X_MOVE_AMPLITUDE=0.0;
+ Y_MOVE_AMPLITUDE=0.0;
+ A_MOVE_AMPLITUDE=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[BIOLOID_WALK_CNTRL]&=(~WALK_PHASE);
+ ram_data[BIOLOID_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[BIOLOID_WALK_CNTRL]&=(~WALK_PHASE);
+ ram_data[BIOLOID_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[BIOLOID_WALK_CNTRL]&=~WALK_STATUS;
+ else
+ {
+ X_MOVE_AMPLITUDE=0.0;
+ Y_MOVE_AMPLITUDE=0.0;
+ A_MOVE_AMPLITUDE=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[BIOLOID_WALK_CNTRL]&=(~WALK_PHASE);
+ ram_data[BIOLOID_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-=BIOLOID_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,PI*1.5,-m_X_Move_Amplitude*m_Arm_Swing_Gain,0);
+ angle[13]=wsin(m_Time,m_PeriodTime,PI*1.5,m_X_Move_Amplitude*m_Arm_Swing_Gain,0);
+ }
+
+ if(ram_data[BIOLOID_WALK_CNTRL]&WALK_STATUS)
+ {
+ m_Time += walking_period;
+ if(m_Time >= m_PeriodTime)
+ m_Time = 0;
+ }
+
+ // Compute angles with the inverse kinematics
+ if((bioloid_leg_ik(&angle[0],ep[0],ep[1],ep[2],ep[3],ep[4],ep[5])!=1) ||
+ (bioloid_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/4.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/4.0))/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]=((180.0*angle[12])/PI)*65536.0;
+ if(manager_get_module(L_SHOULDER_PITCH)==MM_WALKING)
+ manager_current_angles[L_SHOULDER_PITCH]=((-180.0*angle[13])/PI)*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(BIOLOID_WALK_CNTRL,address,length))
+ {
+ if(data[BIOLOID_WALK_CNTRL-address]&WALK_START)
+ walking_start();
+ if(data[BIOLOID_WALK_CNTRL-address]&WALK_STOP)
+ walking_stop();
+ }
+ // 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];
+}
+