diff --git a/include/bioloid_kinematics.h b/include/bioloid_kinematics.h
index 21662e4eafb58a278a58182ca1c6ea9853e18636..feb034bf478e9be074ba995068179509ef0766fa 100755
--- a/include/bioloid_kinematics.h
+++ b/include/bioloid_kinematics.h
@@ -11,6 +11,7 @@ extern "C" {
#define BIOLOID_PELVIS_LENGTH 29.0 //mm
#define BIOLOID_THIGH_LENGTH 76.388 //mm
#define BIOLOID_CALF_LENGTH 76.388 //mm
+#define BIOLOID_PITCH_OFFSET 0.1910 //rad
#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
diff --git a/include/eeprom_init.h b/include/eeprom_init.h
new file mode 100644
index 0000000000000000000000000000000000000000..af23b20a95c60cfaf2ee631184aa90a8eb8919f4
--- /dev/null
+++ b/include/eeprom_init.h
@@ -0,0 +1,79 @@
+#ifndef __EEPROM_INIT_H
+#define __EEPROM_INIT_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "adc_dma.h"
+
+#define DEFAULT_DEVICE_MODEL 0x7300
+#define DEFAULT_FIRMWARE_VERSION 0x0001
+#define DEFAULT_DEVICE_ID 0x0002
+#define DEFAULT_BAUDRATE 0x0001
+#define DEFAULT_RETURN_DELAY 0x0000
+#define DEFAULT_MM_PERIOD 0x1E78
+#define DEFAULT_BAL_KNEE_GAIN 0x04BE // 1/54 in fixed point format 0|16
+#define DEFAULT_BAL_ANKLE_ROLL_GAIN 0x0CCD // 1/20
+#define DEFAULT_BAL_ANKLE_PITCH_GAIN 0x0E39 // 1/18
+#define DEFAULT_BAL_HIP_ROLL_GAIN 0x0666 // 1/40
+#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 // 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
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __EEPROM_INIT_H */
+
diff --git a/src/eeprom.c b/src/eeprom.c
index 48aa5e5c3f1ae341c3775a95db66160ffba1f491..35c1d2266ea43a31a5201dbb41b2d73879531062 100755
--- a/src/eeprom.c
+++ b/src/eeprom.c
@@ -41,74 +41,11 @@
/* Includes ------------------------------------------------------------------*/
#include "eeprom.h"
+#include "eeprom_init.h"
#include "bioloid_registers.h"
-#include "adc_dma.h"
/* Private typedef -----------------------------------------------------------*/
/* Private define ------------------------------------------------------------*/
-#define DEFAULT_DEVICE_MODEL 0x7300
-#define DEFAULT_FIRMWARE_VERSION 0x0001
-#define DEFAULT_DEVICE_ID 0x0002
-#define DEFAULT_BAUDRATE 0x0001
-#define DEFAULT_RETURN_DELAY 0x0000
-#define DEFAULT_MM_PERIOD 0x1E78
-#define DEFAULT_BAL_KNEE_GAIN 0x04BE // 1/54 in fixed point format 0|16
-#define DEFAULT_BAL_ANKLE_ROLL_GAIN 0x0CCD // 1/20
-#define DEFAULT_BAL_ANKLE_PITCH_GAIN 0x0E39 // 1/18
-#define DEFAULT_BAL_HIP_ROLL_GAIN 0x0666 // 1/40
-#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 // 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 ---------------------------------------------------------*/
diff --git a/src/walking.c b/src/walking.c
index 127f736f25a77ab4552304a98d3d8de89ced770b..2594524e85c2c50d6a02186ce29e392732f1cbc2 100755
--- a/src/walking.c
+++ b/src/walking.c
@@ -3,7 +3,6 @@
#include "bioloid_math.h"
#include "ram.h"
#include <math.h>
-#include <stdio.h>
enum {PHASE0=0x00,PHASE1=0x40,PHASE2=0x80,PHASE3=0xC0};
@@ -115,7 +114,7 @@ void update_param_move()
static float current_y_amplitude=0;
static float current_a_amplitude=0;
- target_x_amplitude=((float)ram_data[BIOLOID_WALK_STEP_FW_BW]);
+ target_x_amplitude=((float)((int8_t)ram_data[BIOLOID_WALK_STEP_FW_BW]));
// change longitudinal and transversal velocities to get to the target ones
if(current_x_amplitude<target_x_amplitude)
{
@@ -133,7 +132,7 @@ void update_param_move()
m_X_Swap_Amplitude=current_x_amplitude*((float)ram_data[BIOLOID_WALK_STEP_FW_BW_RATIO])/256.0;
// Right/Left
- target_y_amplitude=((float)ram_data[BIOLOID_WALK_STEP_LEFT_RIGHT]);
+ target_y_amplitude=((float)((int8_t)ram_data[BIOLOID_WALK_STEP_LEFT_RIGHT]));
if(current_y_amplitude<target_y_amplitude)
{
current_y_amplitude+=(((float)ram_data[BIOLOID_WALK_MAX_VEL])*m_PeriodTime)/1000.0;
@@ -159,7 +158,7 @@ void update_param_move()
m_Z_Swap_Amplitude_Shift=m_Z_Swap_Amplitude;
// Direction
- target_a_amplitude=((float)ram_data[BIOLOID_WALK_STEP_DIRECTION]);
+ target_a_amplitude=((float)((int8_t)ram_data[BIOLOID_WALK_STEP_DIRECTION]))/8.0;
if(current_a_amplitude<target_a_amplitude)
{
current_a_amplitude+=(((float)ram_data[BIOLOID_WALK_MAX_ROT_VEL])*m_PeriodTime)/8000.0;
@@ -450,11 +449,11 @@ void walking_process(void)
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;
+ manager_current_angles[R_HIP_PITCH]=((180.0*(angle[2]-BIOLOID_PITCH_OFFSET))/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;
+ manager_current_angles[R_ANKLE_PITCH]=((-180.0*(angle[4]+BIOLOID_PITCH_OFFSET))/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)
@@ -462,11 +461,11 @@ void walking_process(void)
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;
+ manager_current_angles[L_HIP_PITCH]=((-180.0*(angle[8]-BIOLOID_PITCH_OFFSET))/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;
+ manager_current_angles[L_ANKLE_PITCH]=((180.0*(angle[10]+BIOLOID_PITCH_OFFSET))/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)