Skip to content
GitLab
Commit 7550a739 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Taken into account the differences between the Darwin and Bioloid robots in the walking algorithm. 

Chnaged the way the walking commands are given.
parent 9fe788e8
Hide whitespace changes
Inline Side-by-side
src/walking.c
View file @ 7550a739
......@@ -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
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment