diff --git a/src/walking.c b/src/walking.c
index 86f808d9813a098abfe2a2c252f40892d4faaed1..127f736f25a77ab4552304a98d3d8de89ced770b 100755
--- a/src/walking.c
+++ b/src/walking.c
@@ -3,13 +3,11 @@
#include "bioloid_math.h"
#include "ram.h"
#include <math.h>
+#include <stdio.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
@@ -80,7 +78,7 @@ 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_PeriodTime=((float)((int16_t)(ram_data[BIOLOID_WALK_PERIOD_TIME_L]+(ram_data[BIOLOID_WALK_PERIOD_TIME_H]<<8))));
m_SSP_Ratio=1.0-((float)ram_data[BIOLOID_WALK_DSP_RATIO])/256.0;
m_SSP_Time=m_PeriodTime*m_SSP_Ratio;
@@ -103,45 +101,50 @@ void update_param_time()
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_Offset=((float)ram_data[BIOLOID_WALK_PELVIS_OFFSET])*PI/1440.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()
{
+ 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)ram_data[BIOLOID_WALK_STEP_FW_BW]);
// change longitudinal and transversal velocities to get to the target ones
- if(current_x_amplitude<X_MOVE_AMPLITUDE)
+ if(current_x_amplitude<target_x_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;
+ if(current_x_amplitude>target_x_amplitude)
+ current_x_amplitude=target_x_amplitude;
}
- else if(current_x_amplitude>X_MOVE_AMPLITUDE)
+ else if(current_x_amplitude>target_x_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;
+ 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[BIOLOID_WALK_STEP_FW_BW_RATIO])/256.0;
// Right/Left
- if(current_y_amplitude<Y_MOVE_AMPLITUDE)
+ target_y_amplitude=((float)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;
- if(current_y_amplitude>Y_MOVE_AMPLITUDE)
- current_y_amplitude=Y_MOVE_AMPLITUDE;
+ if(current_y_amplitude>target_y_amplitude)
+ current_y_amplitude=target_y_amplitude;
}
- else if(current_y_amplitude>Y_MOVE_AMPLITUDE)
+ else if(current_y_amplitude>target_y_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;
+ 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)
@@ -156,17 +159,18 @@ void update_param_move()
m_Z_Swap_Amplitude_Shift=m_Z_Swap_Amplitude;
// Direction
- if(current_a_amplitude<A_MOVE_AMPLITUDE)
+ target_a_amplitude=((float)ram_data[BIOLOID_WALK_STEP_DIRECTION]);
+ if(current_a_amplitude<target_a_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(current_a_amplitude>target_a_amplitude)
+ current_a_amplitude=target_a_amplitude;
}
- else if(current_a_amplitude>A_MOVE_AMPLITUDE)
+ else if(current_a_amplitude>target_a_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(current_a_amplitude<target_a_amplitude)
+ current_a_amplitude=target_a_amplitude;
}
if(A_MOVE_AIM_ON==0x00)
@@ -195,7 +199,7 @@ void update_param_balance()
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;
+ m_Hip_Pitch_Offset = ((float)((int16_t)(ram_data[BIOLOID_WALK_HIP_PITCH_OFF_L]+(ram_data[BIOLOID_WALK_HIP_PITCH_OFF_H]<<8))))/1024.0;
}
// public functions
@@ -275,9 +279,9 @@ void walking_process(void)
ram_data[BIOLOID_WALK_CNTRL]&=(~WALK_STATUS);
else
{
- X_MOVE_AMPLITUDE=0.0;
- Y_MOVE_AMPLITUDE=0.0;
- A_MOVE_AMPLITUDE=0.0;
+ ram_data[BIOLOID_WALK_STEP_FW_BW]=0.0;
+ ram_data[BIOLOID_WALK_STEP_LEFT_RIGHT]=0.0;
+ ram_data[BIOLOID_WALK_STEP_DIRECTION]=0.0;
}
}
}
@@ -299,9 +303,9 @@ void walking_process(void)
ram_data[BIOLOID_WALK_CNTRL]&=~WALK_STATUS;
else
{
- X_MOVE_AMPLITUDE=0.0;
- Y_MOVE_AMPLITUDE=0.0;
- A_MOVE_AMPLITUDE=0.0;
+ ram_data[BIOLOID_WALK_STEP_FW_BW]=0.0;
+ ram_data[BIOLOID_WALK_STEP_LEFT_RIGHT]=0.0;
+ ram_data[BIOLOID_WALK_STEP_DIRECTION]=0.0;
}
}
}
@@ -424,8 +428,8 @@ void walking_process(void)
}
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);
+ 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[BIOLOID_WALK_CNTRL]&WALK_STATUS)
@@ -444,7 +448,7 @@ void walking_process(void)
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;
+ 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)
@@ -452,23 +456,23 @@ void walking_process(void)
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;
+ 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;
+ 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])/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;
+ 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;
+ manager_current_angles[R_SHOULDER_PITCH]=(angle[12]-90.0)*65536.0;
if(manager_get_module(L_SHOULDER_PITCH)==MM_WALKING)
- manager_current_angles[L_SHOULDER_PITCH]=((-180.0*angle[13])/PI)*65536.0;
+ manager_current_angles[L_SHOULDER_PITCH]=(-angle[13]+90.0)*65536.0;
}
// operation functions