diff --git a/dynamixel_manager/include/modules/motion_manager.h b/dynamixel_manager/include/modules/motion_manager.h
index 59ef8ffd3c0529094abdf8d2fb14ce052d0a4468..26a3e9a11315744a7c225d7d4a643960f5c0b30f 100644
--- a/dynamixel_manager/include/modules/motion_manager.h
+++ b/dynamixel_manager/include/modules/motion_manager.h
@@ -15,7 +15,7 @@ extern "C" {
typedef enum {MM_NONE = 7,
MM_ACTION = 0,
- MM_WALKING = 1,
+ MM_WALK = 1,
MM_JOINTS = 2,
MM_HEAD = 3} TModules;
diff --git a/dynamixel_manager/include/modules/walk.h b/dynamixel_manager/include/modules/walk.h
index 819a66b1eb3440efc75e382be3716e118dd0d256..09131c27c4ea20d8a700bae2fff59eed826f2b1b 100755
--- a/dynamixel_manager/include/modules/walk.h
+++ b/dynamixel_manager/include/modules/walk.h
@@ -14,6 +14,76 @@ typedef struct{
TMemModule mem_module;
unsigned short int ram_base_address;
unsigned short int eeprom_base_address;
+ // aim control
+ unsigned char aim_on;
+ // internal motion variables
+ float x_swap_phase_shift;
+ float x_swap_amplitude;
+ float x_swap_amplitude_shift;
+ float x_move_phase_shift;
+ float x_move_amplitude;
+ float x_move_amplitude_shift;
+ float y_swap_phase_shift;
+ float y_swap_amplitude;
+ float y_swap_amplitude_shift;
+ float y_move_phase_shift;
+ float y_move_amplitude;
+ float y_move_amplitude_shift;
+ float z_swap_phase_shift;
+ float z_swap_amplitude;
+ float z_swap_amplitude_shift;
+ float z_move_phase_shift;
+ float z_move_amplitude;
+ float z_move_amplitude_shift;
+ float a_move_phase_shift;
+ float a_move_amplitude;
+ float a_move_amplitude_shift;
+ // internal offset variables
+ float hip_pitch_offset;
+ float pelvis_offset;
+ float pelvis_swing;
+ float arm_swing_gain;
+ float x_offset;
+ float y_offset;
+ float z_offset;
+ float r_offset;
+ float p_offset;
+ float a_offset;
+ // internal time variables
+ float time;
+ float period_time;
+ float x_swap_period_time;
+ float x_move_period_time;
+ float y_swap_period_time;
+ float y_move_period_time;
+ float z_swap_period_time;
+ float z_move_period_time;
+ float a_move_period_time;
+ float ssp_time_start_left;
+ float ssp_time_end_left;
+ float ssp_time_start_right;
+ float ssp_time_end_right;
+ float phase_time1;
+ float phase_time2;
+ float phase_time3;
+ // control variables
+ unsigned char running;
+ float walk_period;
+ unsigned char (*leg_ik_function)(float *out, float x, float y, float z, float a, float b, float c);
+ unsigned char right_hip_roll_servo_id;
+ unsigned char right_hip_yaw_servo_id;
+ unsigned char right_hip_pitch_servo_id;
+ unsigned char right_knee_servo_id;
+ unsigned char right_ankle_pitch_servo_id;
+ unsigned char right_ankle_roll_servo_id;
+ unsigned char right_shoulder_pitch_servo_id;
+ unsigned char left_hip_roll_servo_id;
+ unsigned char left_hip_yaw_servo_id;
+ unsigned char left_hip_pitch_servo_id;
+ unsigned char left_knee_servo_id;
+ unsigned char left_ankle_pitch_servo_id;
+ unsigned char left_ankle_roll_servo_id;
+ unsigned char left_shoulder_pitch_servo_id;
}TWalkMModule;
// public functions
diff --git a/dynamixel_manager/include/modules/walk_registers.h b/dynamixel_manager/include/modules/walk_registers.h
index 8d2ee7c8a2bbb512950a7f4d2f4e7998968653fd..ac0ef5d92899a63abfdec010decba3ca0cae9217 100644
--- a/dynamixel_manager/include/modules/walk_registers.h
+++ b/dynamixel_manager/include/modules/walk_registers.h
@@ -3,7 +3,7 @@
#define RAM_WALK_LENGTH 4
-#define WALK_CNTRL 0// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+#define WALK_CONTROL 0// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
// current phase walking stop walking start walking
#define WALK_START 0x01
#define WALK_STOP 0x02
@@ -13,7 +13,7 @@
#define WALK_STEP_LEFT_RIGHT 2
#define WALK_STEP_DIRECTION 3
-#define EEPROM_WALK_LENGTH 19
+#define EEPROM_WALK_LENGTH 33
#define WALK_X_OFFSET 0
#define WALK_Y_OFFSET 1
@@ -32,8 +32,23 @@
#define WALK_ARM_SWING_GAIN 16
#define WALK_MAX_VEL 17
#define WALK_MAX_ROT_VEL 18
+#define WALK_R_HIP_YAW_SERVO_ID 19
+#define WALK_R_HIP_ROLL_SERVO_ID 20
+#define WALK_R_HIP_PITCH_SERVO_ID 21
+#define WALK_R_KNEE_SERVO_ID 22
+#define WALK_R_ANKLE_PITCH_SERVO_ID 23
+#define WALK_R_ANKLE_ROLL_SERVO_ID 24
+#define WALK_R_SHOULDER_PITCH_SERVO_ID 25
+#define WALK_L_HIP_YAW_SERVO_ID 26
+#define WALK_L_HIP_ROLL_SERVO_ID 27
+#define WALK_L_HIP_PITCH_SERVO_ID 28
+#define WALK_L_KNEE_SERVO_ID 29
+#define WALK_L_ANKLE_PITCH_SERVO_ID 30
+#define WALK_L_ANKLE_ROLL_SERVO_ID 31
+#define WALK_L_SHOULDER_PITCH_SERVO_ID 32
-#define walk_eeprom_data(name,section_name,base_address,DEFAULT_X_OFFSET,DEFAULT_Y_OFFSET,DEFAULT_Z_OFFSET,DEFAULT_ROLL_OFFSET,DEFAULT_PITCH_OFFSET,DEFAULT_YAW_OFFSET,DEFAULT_HIP_PITCH_OFFSET,DEFAULT_PERIOD_TIME,DEFAULT_DSP_RATIO,DEFAULT_STEP_FW_BW_RATIO,DEFAULT_FOOT_HEIGHT,DEFAULT_SWING_RIGHT_LEFT,DEFAULT_SWING_TOP_DOWN,DEFAULT_PELVIS_OFFSET,DEFAULT_ARM_SWING_GAIN,DEFAULT_MAX_VEL,DEFAULT_MAX_ROT_VEL) \
+
+#define walk_eeprom_data(name,section_name,base_address,DEFAULT_X_OFFSET,DEFAULT_Y_OFFSET,DEFAULT_Z_OFFSET,DEFAULT_ROLL_OFFSET,DEFAULT_PITCH_OFFSET,DEFAULT_YAW_OFFSET,DEFAULT_HIP_PITCH_OFFSET,DEFAULT_PERIOD_TIME,DEFAULT_DSP_RATIO,DEFAULT_STEP_FW_BW_RATIO,DEFAULT_FOOT_HEIGHT,DEFAULT_SWING_RIGHT_LEFT,DEFAULT_SWING_TOP_DOWN,DEFAULT_PELVIS_OFFSET,DEFAULT_ARM_SWING_GAIN,DEFAULT_MAX_VEL,DEFAULT_MAX_ROT_VEL,DEFAULT_R_HIP_YAW_SERVO_ID,DEFAULT_R_HIP_ROLL_SERVO_ID,DEFAULT_R_HIP_PITCH_SERVO_ID,DEFAULT_R_KNEE_SERVO_ID,DEFAULT_R_ANKLE_PITCH_SERVO_ID,DEFAULT_R_ANKLE_ROLL_SERVO_ID,DEFAULT_R_SHOULDER_PITCH_SERVO_ID,DEFAULT_L_HIP_YAW_SERVO_ID,DEFAULT_L_HIP_ROLL_SERVO_ID,DEFAULT_L_HIP_PITCH_SERVO_ID,DEFAULT_L_KNEE_SERVO_ID,DEFAULT_L_ANKLE_PITCH_SERVO_ID,DEFAULT_L_ANKLE_ROLL_SERVO_ID,DEFAULT_L_SHOULDER_PITCH_SERVO_ID) \
unsigned short int name##_eeprom_data[] __attribute__ ((section (section_name))) __attribute__ ((aligned (4)))= {\
DEFAULT_X_OFFSET,base_address+WALK_X_OFFSET, \
DEFAULT_Y_OFFSET,base_address+WALK_Y_OFFSET, \
@@ -53,7 +68,21 @@ unsigned short int name##_eeprom_data[] __attribute__ ((section (section_name)))
DEFAULT_PELVIS_OFFSET,base_address+WALK_PELVIS_OFFSET, \
DEFAULT_ARM_SWING_GAIN,base_address+WALK_ARM_SWING_GAIN, \
DEFAULT_MAX_VEL,base_address+WALK_MAX_VEL, \
- DEFAULT_MAX_ROT_VEL,base_address+WALK_MAX_ROT_VEL};
+ DEFAULT_MAX_ROT_VEL,base_address+WALK_MAX_ROT_VEL \
+ DEFAULT_R_HIP_YAW_SERVO_ID,base_address+WALK_R_HIP_YAW_SERVO_ID \
+ DEFAULT_R_HIP_ROLL_SERVO_ID,base_address+WALK_R_HIP_ROLL_SERVO_ID \
+ DEFAULT_R_HIP_PITCH_SERVO_ID,base_address+WALK_R_HIP_PITCH_SERVO_ID \
+ DEFAULT_R_KNEE_SERVO_ID,base_address+WALK_R_KNEE_SERVO_ID \
+ DEFAULT_R_ANKLE_PITCH_SERVO_ID,base_address+WALK_R_ANKLE_PITCH_SERVO_ID \
+ DEFAULT_R_ANKLE_ROLL_SERVO_ID,base_address+WALK_R_ANKLE_ROLL_SERVO_ID \
+ DEFAULT_R_SHOULDER_PITCH_SERVO_ID,base_address+WALK_R_SHOULDER_PITCH_SERVO_ID \
+ DEFAULT_L_HIP_YAW_SERVO_ID,base_address+WALK_L_HIP_YAW_SERVO_ID \
+ DEFAULT_L_HIP_ROLL_SERVO_ID,base_address+WALK_L_HIP_ROLL_SERVO_ID \
+ DEFAULT_L_HIP_PITCH_SERVO_ID,base_address+WALK_L_HIP_PITCH_SERVO_ID \
+ DEFAULT_L_KNEE_SERVO_ID,base_address+WALK_L_KNEE_SERVO_ID \
+ DEFAULT_L_ANKLE_PITCH_SERVO_ID,base_address+WALK_L_ANKLE_PITCH_SERVO_ID \
+ DEFAULT_L_ANKLE_ROLL_SERVO_ID,base_address+WALK_L_ANKLE_ROLL_SERVO_ID \
+ DEFAULT_L_SHOULDER_PITCH_SERVO_ID,base_address+WALK_L_SHOULDER_PITCH_SERVO_ID};
#endif
diff --git a/dynamixel_manager/src/modules/walk.c b/dynamixel_manager/src/modules/walk.c
index 57277dec02844b3cb7f8eb400228833f35e35f30..cf3a3edb4e5b276ebe8329e43be136a960c2b11d 100755
--- a/dynamixel_manager/src/modules/walk.c
+++ b/dynamixel_manager/src/modules/walk.c
@@ -1,111 +1,110 @@
-#include "walking.h"
-#include "darwin_kinematics.h"
-#include "darwin_math.h"
+#include "walk.h"
+#include "walk_registers.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
+
+void walk_read_cmd(TWalkMModule *module,unsigned short int address,unsigned short int length,unsigned char *data)
+{
+ ram_read_table(module->memory,address,length,data);
+}
+
+void walk_write_cmd(TWalkMModule *module,unsigned short int address,unsigned short int length,unsigned char *data)
+{
+ unsigned short int i,ram_offset,eeprom_offset;
+
+ // walk control
+ ram_offset=address-module->ram_base_address;
+ if(ram_in_range(module->ram_base_address+WALK_CONTROL,address,length))
+ {
+ if(data[WALK_CONTROL-ram_offset]&WALK_START)
+ walk_start(module);
+ if(data[WALK_CONTROL-ram_offset]&WALK_STOP)
+ walk_stop(module);
+ }
+ if(ram_in_range(module->ram_base_address+WALK_STEP_FW_BW,address,length))
+ module->memory->data[module->ram_base_address+WALK_STEP_FW_BW]=data[WALK_STEP_FW_BW-ram_offset];
+ if(ram_in_range(module->ram_base_address+WALK_STEP_LEFT_RIGHT,address,length))
+ module->memory->data[module->ram_base_address+WALK_STEP_LEFT_RIGHT]=data[WALK_STEP_LEFT_RIGHT-ram_offset];
+ if(ram_in_range(module->ram_base_address+WALK_STEP_DIRECTION,address,length))
+ module->memory->data[module->ram_base_address+WALK_STEP_DIRECTION]=data[WALK_STEP_DIRECTION-ram_offset];
+ // walk parameters
+ eeprom_offset=address-module->eeprom_base_address;
+ if(ram_in_range(module->ram_base_address+WALK_R_HIP_ROLL_SERVO_ID,address,length))
+ module->right_hip_roll_servo_id=data[WALK_R_HIP_ROLL_SERVO_ID-eeprom_offset];
+ if(ram_in_range(module->ram_base_address+WALK_R_HIP_YAW_SERVO_ID,address,length))
+ module->right_hip_yaw_servo_id=data[WALK_R_HIP_YAW_SERVO_ID-eeprom_offset];
+ if(ram_in_range(module->ram_base_address+WALK_R_HIP_PITCH_SERVO_ID,address,length))
+ module->right_hip_pitch_servo_id=data[WALK_R_HIP_PITCH_SERVO_ID-eeprom_offset];
+ if(ram_in_range(module->ram_base_address+WALK_R_KNEE_SERVO_ID,address,length))
+ module->right_knee_servo_id=data[WALK_R_KNEE_SERVO_ID-eeprom_offset];
+ if(ram_in_range(module->ram_base_address+WALK_R_ANKLE_PITCH_SERVO_ID,address,length))
+ module->right_ankle_pitch_servo_id=data[WALK_R_ANKLE_PITCH_SERVO_ID-eeprom_offset];
+ if(ram_in_range(module->ram_base_address+WALK_R_ANKLE_ROLL_SERVO_ID,address,length))
+ module->right_ankle_roll_servo_id=data[WALK_R_ANKLE_ROLL_SERVO_ID-eeprom_offset];
+ if(ram_in_range(module->ram_base_address+WALK_R_SHOULDER_PITCH_SERVO_ID,address,length))
+ module->right_shoulder_pitch_servo_id=data[WALK_R_SHOULDER_PITCH_SERVO_ID-eeprom_offset];
+ if(ram_in_range(module->ram_base_address+WALK_L_HIP_ROLL_SERVO_ID,address,length))
+ module->left_hip_roll_servo_id=data[WALK_L_HIP_ROLL_SERVO_ID-eeprom_offset];
+ if(ram_in_range(module->ram_base_address+WALK_L_HIP_YAW_SERVO_ID,address,length))
+ module->left_hip_yaw_servo_id=data[WALK_L_HIP_YAW_SERVO_ID-eeprom_offset];
+ if(ram_in_range(module->ram_base_address+WALK_L_HIP_PITCH_SERVO_ID,address,length))
+ module->left_hip_pitch_servo_id=data[WALK_L_HIP_PITCH_SERVO_ID-eeprom_offset];
+ if(ram_in_range(module->ram_base_address+WALK_L_KNEE_SERVO_ID,address,length))
+ module->left_knee_servo_id=data[WALK_L_KNEE_SERVO_ID-eeprom_offset];
+ if(ram_in_range(module->ram_base_address+WALK_L_ANKLE_PITCH_SERVO_ID,address,length))
+ module->left_ankle_pitch_servo_id=data[WALK_L_ANKLE_PITCH_SERVO_ID-eeprom_offset];
+ if(ram_in_range(module->ram_base_address+WALK_L_ANKLE_ROLL_SERVO_ID,address,length))
+ module->left_ankle_roll_servo_id=data[WALK_L_ANKLE_ROLL_SERVO_ID-eeprom_offset];
+ if(ram_in_range(module->ram_base_address+WALK_L_SHOULDER_PITCH_SERVO_ID,address,length))
+ module->left_shoulder_pitch_servo_id=data[WALK_L_SHOULDER_PITCH_SERVO_ID-eeprom_offset];
+ for(i=0;i<EEPROM_WALK_LENGTH;i++)
+ if(ram_in_range(module->eeprom_base_address+i,address,length))
+ module->memory->data[module->eeprom_base_address+i]=data[i-eeprom_offset];
+}
+
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;
+ return mag*sin(((2.0*3.14159*time)/period)-period_shift)+mag_shift;
}
-void update_param_time()
+void update_param_time(TWalkMModule *walk)
{
- float m_SSP_Ratio;
- float m_SSP_Time;
+ float ssp_ratio;
+ float 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;
+ walk->period_time=((float)((short int)(walk->memory->data[walk->eeprom_base_address+WALK_PERIOD_TIME]+(walk->memory->data[walk->eeprom_base_address+WALK_PERIOD_TIME+1]<<8))));
+ ssp_ratio=1.0-((float)walk->memory->data[walk->eeprom_base_address+WALK_DSP_RATIO])/256.0;
- m_SSP_Time=m_PeriodTime*m_SSP_Ratio;
+ ssp_time=walk->period_time*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;
+ walk->x_swap_period_time=walk->period_time/2.0;
+ walk->x_move_period_time=ssp_time;
+ walk->y_swap_period_time=walk->period_time;
+ walk->y_move_period_time=ssp_time;
+ walk->z_swap_period_time=walk->period_time/2.0;
+ walk->z_move_period_time=ssp_time/2.0;
+ walk->a_move_period_time=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;
+ walk->ssp_time_start_left=(1-ssp_ratio)*walk->period_time/4.0;
+ walk->ssp_time_end_left=(1+ssp_ratio)*walk->period_time/4.0;
+ walk->ssp_time_start_right=(3-ssp_ratio)*walk->period_time/4.0;
+ walk->ssp_time_end_right=(3+ssp_ratio)*walk->period_time/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;
+ walk->phase_time1=(walk->ssp_time_end_left+walk->ssp_time_start_left)/2.0;
+ walk->phase_time2=(walk->ssp_time_start_right+walk->ssp_time_end_left)/2.0;
+ walk->phase_time3=(walk->ssp_time_end_right+walk->ssp_time_start_right)/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;
+ walk->pelvis_offset=((float)walk->memory->data[walk->eeprom_base_address+WALK_PELVIS_OFFSET])*3.14159/1440.0;
+ walk->pelvis_swing=walk->pelvis_offset*0.35;
+ walk->arm_swing_gain=((float)walk->memory->data[walk->eeprom_base_address+WALK_ARM_SWING_GAIN])/32.0;
}
-void update_param_move()
+void update_param_move(TWalkMModule *walk)
{
float target_x_amplitude;
float target_y_amplitude;
@@ -114,277 +113,221 @@ void update_param_move()
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]));
+ target_x_amplitude=((float)((char)walk->memory->data[walk->ram_base_address+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;
+ current_x_amplitude+=(((float)walk->memory->data[walk->eeprom_base_address+WALK_MAX_VEL])*walk->period_time)/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;
+ current_x_amplitude-=(((float)walk->memory->data[walk->eeprom_base_address+WALK_MAX_VEL])*walk->period_time)/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;
+ walk->x_move_amplitude=current_x_amplitude;
+ walk->x_swap_amplitude=current_x_amplitude*((float)walk->memory->data[walk->eeprom_base_address+WALK_STEP_FW_BW_RATIO])/256.0;
// Right/Left
- target_y_amplitude=((float)((int8_t)ram_data[DARWIN_WALK_STEP_LEFT_RIGHT]));
+ target_y_amplitude=((float)((char)walk->memory->data[walk->ram_base_address+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;
+ current_y_amplitude+=(((float)walk->memory->data[walk->eeprom_base_address+WALK_MAX_VEL])*walk->period_time)/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;
+ current_y_amplitude-=(((float)walk->memory->data[walk->eeprom_base_address+WALK_MAX_VEL])*walk->period_time)/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;
+ walk->y_move_amplitude=current_y_amplitude/2.0;
+ if(walk->y_move_amplitude>0)
+ walk->y_move_amplitude_shift=walk->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;
+ walk->y_move_amplitude_shift=-walk->y_move_amplitude;
+ walk->y_swap_amplitude=((float)walk->memory->data[walk->eeprom_base_address+WALK_SWING_RIGHT_LEFT])+walk->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;
+ walk->z_move_amplitude=((float)walk->memory->data[walk->eeprom_base_address+WALK_FOOT_HEIGHT])/2.0;
+ walk->z_move_amplitude_shift=walk->z_move_amplitude / 2.0;
+ walk->z_swap_amplitude=((float)walk->memory->data[walk->eeprom_base_address+WALK_SWING_TOP_DOWN]);
+ walk->z_swap_amplitude_shift=walk->z_swap_amplitude;
// Direction
- target_a_amplitude=((float)((int8_t)ram_data[DARWIN_WALK_STEP_DIRECTION]))/8.0;
+ target_a_amplitude=((float)((char)walk->memory->data[walk->ram_base_address+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;
+ current_a_amplitude+=(((float)walk->memory->data[walk->eeprom_base_address+WALK_MAX_ROT_VEL])*walk->period_time)/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;
+ current_a_amplitude-=(((float)walk->memory->data[walk->eeprom_base_address+WALK_MAX_ROT_VEL])*walk->period_time)/8000.0;
if(current_a_amplitude<target_a_amplitude)
current_a_amplitude=target_a_amplitude;
}
- if(A_MOVE_AIM_ON==0x00)
+ if(walk->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;
+ walk->a_move_amplitude=current_a_amplitude*3.14159/180.0/2.0;
+ if(walk->a_move_amplitude>0)
+ walk->a_move_amplitude_shift=walk->a_move_amplitude;
else
- m_A_Move_Amplitude_Shift=-m_A_Move_Amplitude;
+ walk->a_move_amplitude_shift=-walk->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;
+ walk->a_move_amplitude=-current_a_amplitude*3.14159/180.0/2.0;
+ if(walk->a_move_amplitude>0)
+ walk->a_move_amplitude_shift=-walk->a_move_amplitude;
else
- m_A_Move_Amplitude_Shift=m_A_Move_Amplitude;
+ walk->a_move_amplitude_shift=walk->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)
+void update_param_balance(TWalkMModule *walk)
{
- if(ram_data[DARWIN_WALK_CNTRL]&WALK_STATUS)
- return 0x01;
- else
- return 0x00;
+ walk->x_offset=((float)((char)walk->memory->data[walk->eeprom_base_address+WALK_X_OFFSET]));
+ walk->y_offset=((float)((char)walk->memory->data[walk->eeprom_base_address+WALK_Y_OFFSET]));
+ walk->z_offset=((float)((char)walk->memory->data[walk->eeprom_base_address+WALK_Z_OFFSET]));
+ walk->r_offset=((float)((char)walk->memory->data[walk->eeprom_base_address+WALK_ROLL_OFFSET]))*3.14159/1440.0;// (r_offset/8)*(pi/180)
+ walk->p_offset = ((float)((char)walk->memory->data[walk->eeprom_base_address+WALK_PITCH_OFFSET]))*3.14159/1440.0;// (p_offset/8)*(pi/180)
+ walk->a_offset = ((float)((char)walk->memory->data[walk->eeprom_base_address+WALK_YAW_OFFSET]))*3.14159/1440.0;// (a_offset/8)*(pi/180)
+ walk->hip_pitch_offset = ((float)((short int)(walk->memory->data[walk->eeprom_base_address+WALK_HIP_PITCH_OFFSET]+(walk->memory->data[walk->eeprom_base_address+WALK_HIP_PITCH_OFFSET+1]<<8))))/1024.0;
}
// motion manager interface functions
-void walking_process(void)
+void walk_pre_process(void *module)
{
+ TWalkMModule *walk=(TWalkMModule *)module;
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)
+ if(walk->time==0)
{
- update_param_time();
- ram_data[DARWIN_WALK_CNTRL]&=(~WALK_PHASE);
- ram_data[DARWIN_WALK_CNTRL]|=PHASE0;
- if(m_Ctrl_Running==0x00)
+ update_param_time(walk);
+ walk->memory->data[walk->ram_base_address+WALK_CONTROL]&=(~WALK_CURRENT_PHASE);
+ walk->memory->data[walk->ram_base_address+WALK_CONTROL]|=PHASE0;
+ if(walk->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);
+ if(walk->x_move_amplitude==0 && walk->y_move_amplitude==0 && walk->a_move_amplitude==0)
+ walk->memory->data[walk->ram_base_address+WALK_CONTROL]&=(~WALK_RUNNING);
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;
+ walk->memory->data[walk->ram_base_address+WALK_STEP_FW_BW]=0.0;
+ walk->memory->data[walk->ram_base_address+WALK_STEP_LEFT_RIGHT]=0.0;
+ walk->memory->data[walk->ram_base_address+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))
+ else if(walk->time>=(walk->phase_time1-walk->walk_period/2.0) && walk->time<(walk->phase_time1+walk->walk_period/2.0))
{
- update_param_move();
- ram_data[DARWIN_WALK_CNTRL]&=(~WALK_PHASE);
- ram_data[DARWIN_WALK_CNTRL]|=PHASE1;
+ update_param_move(walk);
+ walk->memory->data[walk->ram_base_address+WALK_CONTROL]&=(~WALK_CURRENT_PHASE);
+ walk->memory->data[walk->ram_base_address+WALK_CONTROL]|=PHASE1;
}
- else if(m_Time>=(m_Phase_Time2-walking_period/2.0) && m_Time<(m_Phase_Time2+walking_period/2.0))
+ else if(walk->time>=(walk->phase_time2-walk->walk_period/2.0) && walk->time<(walk->phase_time2+walk->walk_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)
+ update_param_time(walk);
+ walk->time=walk->phase_time2;
+ walk->memory->data[walk->ram_base_address+WALK_CONTROL]&=(~WALK_CURRENT_PHASE);
+ walk->memory->data[walk->ram_base_address+WALK_CONTROL]|=PHASE2;
+ if(walk->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;
+ if(walk->x_move_amplitude==0 && walk->y_move_amplitude==0 && walk->a_move_amplitude==0)
+ walk->memory->data[walk->ram_base_address+WALK_CONTROL]&=~WALK_RUNNING;
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;
+ walk->memory->data[walk->ram_base_address+WALK_STEP_FW_BW]=0.0;
+ walk->memory->data[walk->ram_base_address+WALK_STEP_LEFT_RIGHT]=0.0;
+ walk->memory->data[walk->ram_base_address+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))
+ else if(walk->time>=(walk->phase_time3-walk->walk_period/2.0) && walk->time<(walk->phase_time3+walk->walk_period/2.0))
{
- update_param_move();
- ram_data[DARWIN_WALK_CNTRL]&=(~WALK_PHASE);
- ram_data[DARWIN_WALK_CNTRL]|=PHASE3;
+ update_param_move(walk);
+ walk->memory->data[walk->ram_base_address+WALK_CONTROL]&=(~WALK_CURRENT_PHASE);
+ walk->memory->data[walk->ram_base_address+WALK_CONTROL]|=PHASE3;
}
- update_param_balance();
+ update_param_balance(walk);
// 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);
+ x_swap=wsin(walk->time,walk->x_swap_period_time,walk->x_swap_phase_shift,walk->x_swap_amplitude,walk->x_swap_amplitude_shift);
+ y_swap=wsin(walk->time,walk->y_swap_period_time,walk->y_swap_phase_shift,walk->y_swap_amplitude,walk->y_swap_amplitude_shift);
+ z_swap=wsin(walk->time,walk->z_swap_period_time,walk->z_swap_phase_shift,walk->z_swap_amplitude,walk->z_swap_amplitude_shift);
a_swap=0;
b_swap=0;
c_swap=0;
- if(m_Time<=m_SSP_Time_Start_L)
+ if(walk->time<=walk->ssp_time_start_left)
{
- 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);
+ x_move_l=wsin(walk->ssp_time_start_left,walk->x_move_period_time,walk->x_move_phase_shift+2.0*3.14159/walk->x_move_period_time*walk->ssp_time_start_left,walk->x_move_amplitude,walk->x_move_amplitude_shift);
+ y_move_l=wsin(walk->ssp_time_start_left,walk->y_move_period_time,walk->y_move_phase_shift+2.0*3.14159/walk->y_move_period_time*walk->ssp_time_start_left,walk->y_move_amplitude,walk->y_move_amplitude_shift);
+ z_move_l=wsin(walk->ssp_time_start_left,walk->z_move_period_time,walk->z_move_phase_shift+2.0*3.14159/walk->z_move_period_time*walk->ssp_time_start_left,walk->z_move_amplitude,walk->z_move_amplitude_shift);
+ c_move_l=wsin(walk->ssp_time_start_left,walk->a_move_period_time,walk->a_move_phase_shift+2.0*3.14159/walk->a_move_period_time*walk->ssp_time_start_left,walk->a_move_amplitude,walk->a_move_amplitude_shift);
+ x_move_r=wsin(walk->ssp_time_start_left,walk->x_move_period_time,walk->x_move_phase_shift+2.0*3.14159/walk->x_move_period_time*walk->ssp_time_start_left,-walk->x_move_amplitude,-walk->x_move_amplitude_shift);
+ y_move_r=wsin(walk->ssp_time_start_left,walk->y_move_period_time,walk->y_move_phase_shift+2.0*3.14159/walk->y_move_period_time*walk->ssp_time_start_left,-walk->y_move_amplitude,-walk->y_move_amplitude_shift);
+ z_move_r=wsin(walk->ssp_time_start_right,walk->z_move_period_time,walk->z_move_phase_shift+2.0*3.14159/walk->z_move_period_time*walk->ssp_time_start_right,walk->z_move_amplitude,walk->z_move_amplitude_shift);
+ c_move_r=wsin(walk->ssp_time_start_left,walk->a_move_period_time,walk->a_move_phase_shift+2.0*3.14159/walk->a_move_period_time*walk->ssp_time_start_left,-walk->a_move_amplitude,-walk->a_move_amplitude_shift);
pelvis_offset_l=0;
pelvis_offset_r=0;
}
- else if(m_Time<=m_SSP_Time_End_L)
+ else if(walk->time<=walk->ssp_time_end_left)
{
- 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);
+ x_move_l=wsin(walk->time,walk->x_move_period_time,walk->x_move_phase_shift+2.0*3.14159/walk->x_move_period_time*walk->ssp_time_start_left,walk->x_move_amplitude,walk->x_move_amplitude_shift);
+ y_move_l=wsin(walk->time,walk->y_move_period_time,walk->y_move_phase_shift+2.0*3.14159/walk->y_move_period_time*walk->ssp_time_start_left,walk->y_move_amplitude,walk->y_move_amplitude_shift);
+ z_move_l=wsin(walk->time,walk->z_move_period_time,walk->z_move_phase_shift+2.0*3.14159/walk->z_move_period_time*walk->ssp_time_start_left,walk->z_move_amplitude,walk->z_move_amplitude_shift);
+ c_move_l=wsin(walk->time,walk->a_move_period_time,walk->a_move_phase_shift+2.0*3.14159/walk->a_move_period_time*walk->ssp_time_start_left,walk->a_move_amplitude,walk->a_move_amplitude_shift);
+ x_move_r=wsin(walk->time,walk->x_move_period_time,walk->x_move_phase_shift+2.0*3.14159/walk->x_move_period_time*walk->ssp_time_start_left,-walk->x_move_amplitude,-walk->x_move_amplitude_shift);
+ y_move_r=wsin(walk->time,walk->y_move_period_time,walk->y_move_phase_shift+2.0*3.14159/walk->y_move_period_time*walk->ssp_time_start_left,-walk->y_move_amplitude,-walk->y_move_amplitude_shift);
+ z_move_r=wsin(walk->ssp_time_start_right,walk->z_move_period_time,walk->z_move_phase_shift+2.0*3.14159/walk->z_move_period_time*walk->ssp_time_start_right,walk->z_move_amplitude,walk->z_move_amplitude_shift);
+ c_move_r=wsin(walk->time,walk->a_move_period_time,walk->a_move_phase_shift+2.0*3.14159/walk->a_move_period_time*walk->ssp_time_start_left,-walk->a_move_amplitude,-walk->a_move_amplitude_shift);
+ pelvis_offset_l=wsin(walk->time,walk->z_move_period_time,walk->z_move_phase_shift+2.0*3.14159/walk->z_move_period_time*walk->ssp_time_start_left,walk->pelvis_swing/2.0,walk->pelvis_swing/2.0);
+ pelvis_offset_r=wsin(walk->time,walk->z_move_period_time,walk->z_move_phase_shift+2.0*3.14159/walk->z_move_period_time*walk->ssp_time_start_left,-walk->pelvis_offset/2.0,-walk->pelvis_offset/2.0);
}
- else if(m_Time<=m_SSP_Time_Start_R)
+ else if(walk->time<=walk->ssp_time_start_right)
{
- 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);
+ x_move_l=wsin(walk->ssp_time_end_left,walk->x_move_period_time,walk->x_move_phase_shift+2.0*3.14159/walk->x_move_period_time*walk->ssp_time_start_left,walk->x_move_amplitude,walk->x_move_amplitude_shift);
+ y_move_l=wsin(walk->ssp_time_end_left,walk->y_move_period_time,walk->y_move_phase_shift+2.0*3.14159/walk->y_move_period_time*walk->ssp_time_start_left,walk->y_move_amplitude,walk->y_move_amplitude_shift);
+ z_move_l=wsin(walk->ssp_time_end_left,walk->z_move_period_time,walk->z_move_phase_shift+2.0*3.14159/walk->z_move_period_time*walk->ssp_time_start_left,walk->z_move_amplitude,walk->z_move_amplitude_shift);
+ c_move_l=wsin(walk->ssp_time_end_left,walk->a_move_period_time,walk->a_move_phase_shift+2.0*3.14159/walk->a_move_period_time*walk->ssp_time_start_left,walk->a_move_amplitude,walk->a_move_amplitude_shift);
+ x_move_r=wsin(walk->ssp_time_end_left,walk->x_move_period_time,walk->x_move_phase_shift+2.0*3.14159/walk->x_move_period_time*walk->ssp_time_start_left,-walk->x_move_amplitude,-walk->x_move_amplitude_shift);
+ y_move_r=wsin(walk->ssp_time_end_left,walk->y_move_period_time,walk->y_move_phase_shift+2.0*3.14159/walk->y_move_period_time*walk->ssp_time_start_left,-walk->y_move_amplitude,-walk->y_move_amplitude_shift);
+ z_move_r=wsin(walk->ssp_time_start_right,walk->z_move_period_time,walk->z_move_phase_shift+2.0*3.14159/walk->z_move_period_time*walk->ssp_time_start_right,walk->z_move_amplitude,walk->z_move_amplitude_shift);
+ c_move_r=wsin(walk->ssp_time_end_left,walk->a_move_period_time,walk->a_move_phase_shift+2.0*3.14159/walk->a_move_period_time*walk->ssp_time_start_left,-walk->a_move_amplitude,-walk->a_move_amplitude_shift);
pelvis_offset_l=0.0;
pelvis_offset_r=0.0;
}
- else if(m_Time<=m_SSP_Time_End_R)
+ else if(walk->time<=walk->ssp_time_end_right)
{
- 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);
+ x_move_l=wsin(walk->time,walk->x_move_period_time,walk->x_move_phase_shift+2.0*3.14159/walk->x_move_period_time*walk->ssp_time_start_right+3.14159,walk->x_move_amplitude,walk->x_move_amplitude_shift);
+ y_move_l=wsin(walk->time,walk->y_move_period_time,walk->y_move_phase_shift+2.0*3.14159/walk->y_move_period_time*walk->ssp_time_start_right+3.14159,walk->y_move_amplitude,walk->y_move_amplitude_shift);
+ z_move_l=wsin(walk->ssp_time_end_left,walk->z_move_period_time,walk->z_move_phase_shift+2.0*3.14159/walk->z_move_period_time*walk->ssp_time_start_left,walk->z_move_amplitude,walk->z_move_amplitude_shift);
+ c_move_l=wsin(walk->time,walk->a_move_period_time,walk->a_move_phase_shift+2.0*3.14159/walk->a_move_period_time*walk->ssp_time_start_right+3.14159,walk->a_move_amplitude,walk->a_move_amplitude_shift);
+ x_move_r=wsin(walk->time,walk->x_move_period_time,walk->x_move_phase_shift+2.0*3.14159/walk->x_move_period_time*walk->ssp_time_start_right+3.14159,-walk->x_move_amplitude,-walk->x_move_amplitude_shift);
+ y_move_r=wsin(walk->time,walk->y_move_period_time,walk->y_move_phase_shift+2.0*3.14159/walk->y_move_period_time*walk->ssp_time_start_right+3.14159,-walk->y_move_amplitude,-walk->y_move_amplitude_shift);
+ z_move_r=wsin(walk->time,walk->z_move_period_time,walk->z_move_phase_shift+2.0*3.14159/walk->z_move_period_time*walk->ssp_time_start_right,walk->z_move_amplitude,walk->z_move_amplitude_shift);
+ c_move_r=wsin(walk->time,walk->a_move_period_time,walk->a_move_phase_shift+2.0*3.14159/walk->a_move_period_time*walk->ssp_time_start_right+3.14159,-walk->a_move_amplitude,-walk->a_move_amplitude_shift);
+ pelvis_offset_l=wsin(walk->time,walk->z_move_period_time,walk->z_move_phase_shift+2.0*3.14159/walk->z_move_period_time*walk->ssp_time_start_right,walk->pelvis_offset/2,walk->pelvis_offset/2);
+ pelvis_offset_r=wsin(walk->time,walk->z_move_period_time,walk->z_move_phase_shift+2.0*3.14159/walk->z_move_period_time*walk->ssp_time_start_right,-walk->pelvis_swing/2,-walk->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);
+ x_move_l=wsin(walk->ssp_time_end_right,walk->x_move_period_time,walk->x_move_phase_shift+2.0*3.14159/walk->x_move_period_time*walk->ssp_time_start_right+3.14159,walk->x_move_amplitude,walk->x_move_amplitude_shift);
+ y_move_l=wsin(walk->ssp_time_end_right,walk->y_move_period_time,walk->y_move_phase_shift+2.0*3.14159/walk->y_move_period_time*walk->ssp_time_start_right+3.14159,walk->y_move_amplitude,walk->y_move_amplitude_shift);
+ z_move_l=wsin(walk->ssp_time_end_left,walk->z_move_period_time,walk->z_move_phase_shift+2.0*3.14159/walk->z_move_period_time*walk->ssp_time_start_left,walk->z_move_amplitude,walk->z_move_amplitude_shift);
+ c_move_l=wsin(walk->ssp_time_end_right,walk->a_move_period_time,walk->a_move_phase_shift+2.0*3.14159/walk->a_move_period_time*walk->ssp_time_start_right+3.14159,walk->a_move_amplitude,walk->a_move_amplitude_shift);
+ x_move_r=wsin(walk->ssp_time_end_right,walk->x_move_period_time,walk->x_move_phase_shift+2.0*3.14159/walk->x_move_period_time*walk->ssp_time_start_right+3.14159,-walk->x_move_amplitude,-walk->x_move_amplitude_shift);
+ y_move_r=wsin(walk->ssp_time_end_right,walk->y_move_period_time,walk->y_move_phase_shift+2.0*3.14159/walk->y_move_period_time*walk->ssp_time_start_right+3.14159,-walk->y_move_amplitude,-walk->y_move_amplitude_shift);
+ z_move_r=wsin(walk->ssp_time_end_right,walk->z_move_period_time,walk->z_move_phase_shift+2.0*3.14159/walk->z_move_period_time*walk->ssp_time_start_right,walk->z_move_amplitude,walk->z_move_amplitude_shift);
+ c_move_r=wsin(walk->ssp_time_end_right,walk->a_move_period_time,walk->a_move_phase_shift+2.0*3.14159/walk->a_move_period_time*walk->ssp_time_start_right+3.14159,-walk->a_move_amplitude,-walk->a_move_amplitude_shift);
pelvis_offset_l=0.0;
pelvis_offset_r=0.0;
}
@@ -393,21 +336,21 @@ void walking_process(void)
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;
+ ep[0]=x_swap+x_move_r+walk->x_offset;
+ ep[1]=y_swap+y_move_r-walk->y_offset / 2.0;
+ ep[2]=z_swap+z_move_r+walk->z_offset;
+ ep[3]=a_swap+a_move_r-walk->r_offset / 2.0;
+ ep[4]=b_swap+b_move_r+walk->p_offset;
+ ep[5]=c_swap+c_move_r-walk->a_offset / 2.0;
+ ep[6]=x_swap+x_move_l+walk->x_offset;
+ ep[7]=y_swap+y_move_l+walk->y_offset / 2.0;
+ ep[8]=z_swap+z_move_l+walk->z_offset;
+ ep[9]=a_swap+a_move_l+walk->r_offset / 2.0;
+ ep[10]=b_swap+b_move_l+walk->p_offset;
+ ep[11]=c_swap+c_move_l+walk->a_offset / 2.0;
// Compute body swing
-/* if(m_Time<=m_SSP_Time_End_L)
+/* if(time<=ssp_time_end_left)
{
m_Body_Swing_Y=-ep[7];
m_Body_Swing_Z=ep[8];
@@ -417,99 +360,154 @@ void walking_process(void)
m_Body_Swing_Y=-ep[1];
m_Body_Swing_Z=ep[2];
}
- m_Body_Swing_Z-=DARWIN_LEG_LENGTH;*/
+ m_Body_Swing_Z-=LEG_LENGTH;*/
// Compute arm swing
- if(m_X_Move_Amplitude==0)
+ if(walk->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);
+ angle[12]=wsin(walk->time,walk->period_time,0.0,-walk->x_move_amplitude*walk->arm_swing_gain,0.0);
+ angle[13]=wsin(walk->time,walk->period_time,0.0,walk->x_move_amplitude*walk->arm_swing_gain,0.0);
}
- if(ram_data[DARWIN_WALK_CNTRL]&WALK_STATUS)
+ if(walk->memory->data[walk->ram_base_address+WALK_CONTROL]&WALK_RUNNING)
{
- m_Time += walking_period;
- if(m_Time >= m_PeriodTime)
- m_Time = 0;
+ walk->time+=walk->walk_period;
+ if(walk->time>=walk->period_time)
+ walk->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
+ if(walk->leg_ik_function!=0x00000000)
+ {
+ if((walk->leg_ik_function(&angle[0],ep[0],ep[1],ep[2],ep[3],ep[4],ep[5])!=1) ||
+ (walk->leg_ik_function(&angle[6],ep[6],ep[7],ep[8],ep[9],ep[10],ep[11])!=1))
+ return;// Do not use angles
+ }
+ else
+ return;
// 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;
+ if(mmanager_get_module(walk->mmodule.manager,walk->right_hip_yaw_servo_id)==MM_WALK)
+ walk->mmodule.manager->servo_values[walk->right_hip_yaw_servo_id].target_angle=((-180.0*angle[0])/3.14159)*65536.0;
+ if(mmanager_get_module(walk->mmodule.manager,walk->right_hip_roll_servo_id)==MM_WALK)
+ walk->mmodule.manager->servo_values[walk->right_hip_roll_servo_id].target_angle=((-180.0*(angle[1]+pelvis_offset_r))/3.14159)*65536.0;
+ if(mmanager_get_module(walk->mmodule.manager,walk->right_hip_pitch_servo_id)==MM_WALK)
+ walk->mmodule.manager->servo_values[walk->right_hip_pitch_servo_id].target_angle=((180.0*angle[2])/3.14159-walk->hip_pitch_offset)*65536.0;
+ if(mmanager_get_module(walk->mmodule.manager,walk->right_knee_servo_id)==MM_WALK)
+ walk->mmodule.manager->servo_values[walk->right_knee_servo_id].target_angle=((180.0*angle[3])/3.14159)*65536.0;
+ if(mmanager_get_module(walk->mmodule.manager,walk->right_ankle_pitch_servo_id)==MM_WALK)
+ walk->mmodule.manager->servo_values[walk->right_ankle_pitch_servo_id].target_angle=((-180.0*angle[4])/3.14159)*65536.0;
+ if(mmanager_get_module(walk->mmodule.manager,walk->right_ankle_roll_servo_id)==MM_WALK)
+ walk->mmodule.manager->servo_values[walk->right_ankle_roll_servo_id].target_angle=((180.0*angle[5])/3.14159)*65536.0;
+ if(mmanager_get_module(walk->mmodule.manager,walk->left_hip_yaw_servo_id)==MM_WALK)
+ walk->mmodule.manager->servo_values[walk->left_hip_yaw_servo_id].target_angle=((-180.0*angle[6])/3.14159)*65536.0;
+ if(mmanager_get_module(walk->mmodule.manager,walk->left_hip_roll_servo_id)==MM_WALK)
+ walk->mmodule.manager->servo_values[walk->left_hip_roll_servo_id].target_angle=((-180.0*(angle[7]+pelvis_offset_l))/3.14159)*65536.0;
+ if(mmanager_get_module(walk->mmodule.manager,walk->left_hip_pitch_servo_id)==MM_WALK)
+ walk->mmodule.manager->servo_values[walk->left_hip_pitch_servo_id].target_angle=((-180.0*angle[8])/3.14159+walk->hip_pitch_offset)*65536.0;
+ if(mmanager_get_module(walk->mmodule.manager,walk->left_knee_servo_id)==MM_WALK)
+ walk->mmodule.manager->servo_values[walk->left_knee_servo_id].target_angle=((-180.0*angle[9])/3.14159)*65536.0;
+ if(mmanager_get_module(walk->mmodule.manager,walk->left_ankle_pitch_servo_id)==MM_WALK)
+ walk->mmodule.manager->servo_values[walk->left_ankle_pitch_servo_id].target_angle=((180.0*angle[10])/3.14159)*65536.0;
+ if(mmanager_get_module(walk->mmodule.manager,walk->left_ankle_roll_servo_id)==MM_WALK)
+ walk->mmodule.manager->servo_values[walk->left_ankle_roll_servo_id].target_angle=((180.0*angle[11])/3.14159)*65536.0;
+ if(mmanager_get_module(walk->mmodule.manager,walk->right_shoulder_pitch_servo_id)==MM_WALK)
+ walk->mmodule.manager->servo_values[walk->right_shoulder_pitch_servo_id].target_angle=(angle[12]-48.345)*65536.0;
+ if(mmanager_get_module(walk->mmodule.manager,walk->left_shoulder_pitch_servo_id)==MM_WALK)
+ walk->mmodule.manager->servo_values[walk->left_shoulder_pitch_servo_id].target_angle=(-angle[13]+41.313)*65536.0;
}
-// operation functions
-uint8_t walking_in_range(unsigned short int address, unsigned short int length)
+TMotionModule *walk_get_module(TWalkMModule *walk)
{
- 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;
+ return &walk->mmodule;
}
-void walking_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
+void walk_set_period(void *module,unsigned short int period_ms)
{
+ TWalkMModule *walk=(TWalkMModule *)module;
+ walk->walk_period=((float)period_ms)/1000000.0;
}
-void walking_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
+void walk_set_module(void *module,unsigned char servo_id)
{
- 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];
}
+// public functions
+unsigned char walk_init(TWalkMModule *walk,TMemory *memory,unsigned short int ram_base_address,unsigned short int eeprom_base_address)
+{
+ // initialize the internal motion variables
+ walk->aim_on=0x00;
+ walk->x_swap_phase_shift=3.14159;
+ walk->x_swap_amplitude_shift=0.0;
+ walk->x_move_phase_shift=3.14159/2.0;
+ walk->x_move_amplitude=0.0;
+ walk->x_move_amplitude_shift=0.0;
+ walk->y_swap_phase_shift=0.0;
+ walk->y_swap_amplitude_shift=0.0;
+ walk->y_move_phase_shift=3.14159/2.0;
+ walk->z_swap_phase_shift=3.14159*3.0/2.0;
+ walk->z_move_phase_shift=3.14159/2.0;
+ walk->a_move_phase_shift=3.14159/2.0;
+
+ // initialize internal control variables
+ walk->time=0;
+ walk->running=0x00;
+ walk->leg_ik_function=0x00000000;
+
+ update_param_time(walk);
+ update_param_move(walk);
+
+ walk_pre_process(walk);
+
+ /* initialize the motion module */
+ mmodule_init(&walk->mmodule);
+ walk->mmodule.id=MM_WALK;
+ walk->mmodule.set_period=walk_set_period;
+ walk->mmodule.set_module=walk_set_module;
+ walk->mmodule.pre_process=walk_pre_process;
+ walk->mmodule.data=walk;
+
+ mem_module_init(&walk->mem_module);
+ walk->mem_module.data=walk;
+ walk->mem_module.write_cmd=(void(*)(void *,unsigned short int, unsigned short int,unsigned char *))walk_write_cmd;
+ walk->mem_module.read_cmd=(void(*)(void *,unsigned short int, unsigned short int,unsigned char *))walk_read_cmd;
+ if(!mem_module_add_ram_segment(&walk->mem_module,ram_base_address,RAM_WALK_LENGTH))
+ return 0x00;
+ if(!mem_module_add_eeprom_segment(&walk->mem_module,eeprom_base_address,EEPROM_WALK_LENGTH))
+ return 0x00;
+ if(!mem_add_module(memory,&walk->mem_module))
+ return 0x00;
+ walk->ram_base_address=ram_base_address;
+ walk->eeprom_base_address=eeprom_base_address;
+ walk->memory=memory;
+
+ return 0x01;
+}
+
+void walk_start(TWalkMModule *walk)
+{
+ walk->memory->data[walk->ram_base_address+WALK_CONTROL]|=WALK_RUNNING;
+ walk->running=0x01;
+}
+
+void walk_stop(TWalkMModule *walk)
+{
+ walk->running=0x00;
+}
+
+unsigned char is_walk(TWalkMModule *walk)
+{
+ if(walk->memory->data[walk->ram_base_address+WALK_CONTROL]&WALK_RUNNING)
+ return 0x01;
+ else
+ return 0x00;
+}
+
+