Skip to content
Snippets Groups Projects
Commit 05bea09c authored by shernand's avatar shernand
Browse files

Added the PID parameters values to the stairs algorithm. 

Changed the coding of the roll and pitch shift parameters.
Modified the upstairs algorithm to work in the real robot.
parent 2c9cf1d6
No related branches found
No related tags found
2 merge requests!5Dynamixel manager,!2Smart charger fw
Hide whitespace changes
Inline Side-by-side
include/eeprom_init.h
+ 2
− 2
View file @ 05bea09c
......@@ -104,8 +104,8 @@ extern "C" {
#define DEFAULT_STAIRS_Z_OVERSHOOT 0x000F // 15 mm
#define DEFAULT_STAIRS_Z_HEIGHT 0x001E // 30 mm
#define DEFAULT_STAIRS_HIP_PITCH_OFF 0x34B6 // 13.18 degrees in fixed point format 6 (1+5) | 10
#define DEFAULT_STAIRS_R_SHIFT 0x0017 // 2.875 degrees in fixed point format 5 (1+4) | 3
#define DEFAULT_STAIRS_P_SHIFT 0x002E // 5.73 degrees in fixed point format 5 (1+4) | 3
#define DEFAULT_STAIRS_R_SHIFT 0x000B // 2.875 degrees in fixed point format 5 (1+5) | 2
#define DEFAULT_STAIRS_P_SHIFT 0x0017 // 5.73 degrees in fixed point format 5 (1+5) | 2
#define DEFAULT_STAIRS_A_SHIFT 0x0045 // 17.19 degrees in fixed point format 6 (1+5) | 2
#define DEFAULT_STAIRS_Y_SPREAD 0x0014 // 20 mm
#define DEFAULT_STAIRS_X_SHIFT_BODY 0x0023 // 35 mm
......
src/stairs.c
+ 53
− 21
View file @ 05bea09c
......@@ -85,8 +85,8 @@ void stairs_start(uint8_t up)
stairs_Y_shift_amplitude=((float)((int8_t)ram_data[DARWIN_STAIRS_Y_SHIFT]));
stairs_X_shift_amplitude=((float)((int8_t)ram_data[DARWIN_STAIRS_X_SHIFT]));
stairs_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;
stairs_R_shift_amplitude=((float)((int8_t)ram_data[DARWIN_STAIRS_R_SHIFT]))*PI/1440.0;// (r_shift/8)*(pi/180)
stairs_P_shift_amplitude=((float)((int8_t)ram_data[DARWIN_STAIRS_P_SHIFT]))*PI/1440.0;// (p_shift/8)*(pi/180)
stairs_R_shift_amplitude=((float)((int8_t)ram_data[DARWIN_STAIRS_R_SHIFT]))*PI/720.0;// (r_shift/4)*(pi/180)
stairs_P_shift_amplitude=((float)((int8_t)ram_data[DARWIN_STAIRS_P_SHIFT]))*PI/720.0;// (p_shift/4)*(pi/180)
stairs_A_shift_amplitude=((float)((int8_t)ram_data[DARWIN_STAIRS_A_SHIFT]))*PI/720.0;// (a_shift/4)*(pi/180)
stairs_Y_spread_amplitude=((float)((int8_t)ram_data[DARWIN_STAIRS_Y_SPREAD]));
stairs_X_shift_body=((float)((int8_t)ram_data[DARWIN_STAIRS_X_SHIFT_BODY]));
......@@ -210,14 +210,14 @@ void stairs_process(void)
ep[0]=stairs_X_Offset-stairs_X_shift_body*delta;
ep[1]=-stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0)-(2*stairs_Y_shift_amplitude)*delta;
ep[2]=stairs_Z_Offset;
ep[3]=stairs_R_Offset-stairs_R_shift_amplitude+stairs_R_shift_amplitude*delta;
ep[4]=stairs_P_Offset+stairs_P_shift_amplitude-(3.0*stairs_P_shift_amplitude)*delta;
ep[3]=stairs_R_Offset-stairs_R_shift_amplitude+2.0*stairs_R_shift_amplitude*delta;
ep[4]=stairs_P_Offset+stairs_P_shift_amplitude-3.0*stairs_P_shift_amplitude*delta;
ep[5]=stairs_A_Offset+stairs_A_shift_amplitude;
ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body*delta;
ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0)-(2*stairs_Y_shift_amplitude)*delta;
ep[8]=stairs_Z_Offset+stairs_Z_stair_height;
ep[9]=stairs_R_Offset-stairs_R_shift_amplitude+stairs_R_shift_amplitude*delta;
ep[10]=stairs_P_Offset+stairs_P_shift_amplitude-(3.0*stairs_P_shift_amplitude)*delta;
ep[9]=stairs_R_Offset-stairs_R_shift_amplitude+2.0*stairs_R_shift_amplitude*delta;
ep[10]=stairs_P_Offset+stairs_P_shift_amplitude-3.0*stairs_P_shift_amplitude*delta;
ep[11]=stairs_A_Offset+stairs_A_shift_amplitude;
ram_data[DARWIN_STAIRS_CNTRL]|=SHIFT_WEIGHT_RIGHT;
}
......@@ -228,13 +228,13 @@ void stairs_process(void)
ep[0]=stairs_X_Offset-stairs_X_shift_body;
ep[1]=-stairs_Y_Offset/2.0-stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0);
ep[2]=stairs_Z_Offset+(stairs_Z_overshoot+stairs_Z_stair_height)*delta;
ep[3]=stairs_R_Offset+stairs_R_shift_amplitude*delta;
ep[3]=stairs_R_Offset+stairs_R_shift_amplitude;
ep[4]=stairs_P_Offset-2.0*stairs_P_shift_amplitude;
ep[5]=stairs_A_Offset+stairs_A_shift_amplitude;
ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body;
ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0);
ep[8]=stairs_Z_Offset+stairs_Z_stair_height-stairs_Z_overshoot*delta;
ep[9]=stairs_R_Offset+stairs_R_shift_amplitude*delta;
ep[8]=stairs_Z_Offset+stairs_Z_stair_height;//-stairs_Z_overshoot*delta;
ep[9]=stairs_R_Offset+stairs_R_shift_amplitude;
ep[10]=stairs_P_Offset-2.0*stairs_P_shift_amplitude;
ep[11]=stairs_A_Offset+stairs_A_shift_amplitude;
ram_data[DARWIN_STAIRS_CNTRL]|=RISE_LEFT_FOOT;
......@@ -251,7 +251,7 @@ void stairs_process(void)
ep[5]=stairs_A_Offset+stairs_A_shift_amplitude-stairs_A_shift_amplitude*delta;
ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body-(stairs_X_shift_amplitude-stairs_X_shift_body)*delta;
ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0)-(stairs_Y_spread_amplitude/2.0)*delta;
ep[8]=stairs_Z_Offset+stairs_Z_stair_height-stairs_Z_overshoot;
ep[8]=stairs_Z_Offset+stairs_Z_stair_height;//-stairs_Z_overshoot;
ep[9]=stairs_R_Offset+stairs_R_shift_amplitude;
ep[10]=stairs_P_Offset-2.0*stairs_P_shift_amplitude+stairs_P_shift_amplitude*delta;
ep[11]=stairs_A_Offset+stairs_A_shift_amplitude-stairs_A_shift_amplitude*delta;
......@@ -269,7 +269,7 @@ void stairs_process(void)
ep[5]=stairs_A_Offset;
ep[6]=stairs_X_Offset;
ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude;
ep[8]=stairs_Z_Offset+stairs_Z_stair_height-stairs_Z_overshoot+stairs_Z_overshoot*delta;
ep[8]=stairs_Z_Offset+stairs_Z_stair_height;//-stairs_Z_overshoot+stairs_Z_overshoot*delta;
ep[9]=stairs_R_Offset+stairs_R_shift_amplitude-stairs_R_shift_amplitude*delta;
ep[10]=stairs_P_Offset-stairs_P_shift_amplitude+stairs_P_shift_amplitude*delta;
ep[11]=stairs_A_Offset;
......@@ -381,13 +381,13 @@ void stairs_process(void)
ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0)-(2*stairs_Y_shift_amplitude)*delta;
ep[2]=stairs_Z_Offset+stairs_Z_stair_height+stairs_Z_overshoot;
ep[3]=stairs_R_Offset-stairs_R_shift_amplitude+stairs_R_shift_amplitude*delta;
ep[4]=stairs_P_Offset+stairs_P_shift_amplitude-(3.0*stairs_P_shift_amplitude)*delta;
ep[4]=stairs_P_Offset+stairs_P_shift_amplitude-(4.0*stairs_P_shift_amplitude)*delta;
ep[5]=stairs_A_Offset+stairs_A_shift_amplitude;
ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body*delta;
ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0)-(2*stairs_Y_shift_amplitude)*delta;
ep[8]=stairs_Z_Offset+stairs_Z_overshoot;
ep[9]=stairs_R_Offset-stairs_R_shift_amplitude+stairs_R_shift_amplitude*delta;
ep[10]=stairs_P_Offset+stairs_P_shift_amplitude-(3.0*stairs_P_shift_amplitude)*delta;
ep[10]=stairs_P_Offset+stairs_P_shift_amplitude-(4.0*stairs_P_shift_amplitude)*delta;
ep[11]=stairs_A_Offset+stairs_A_shift_amplitude;
ram_data[DARWIN_STAIRS_CNTRL]|=SHIFT_WEIGHT_RIGHT;
}
......@@ -399,13 +399,13 @@ void stairs_process(void)
ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0);
ep[2]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height;
ep[3]=stairs_R_Offset+stairs_R_shift_amplitude*delta;
ep[4]=stairs_P_Offset-2.0*stairs_P_shift_amplitude;
ep[4]=stairs_P_Offset-3.0*stairs_P_shift_amplitude;
ep[5]=stairs_A_Offset+stairs_A_shift_amplitude;
ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body;
ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0);
ep[8]=stairs_Z_Offset+stairs_Z_overshoot-stairs_Z_overshoot*delta;
ep[9]=stairs_R_Offset+stairs_R_shift_amplitude*delta;
ep[10]=stairs_P_Offset-2.0*stairs_P_shift_amplitude;
ep[10]=stairs_P_Offset-3.0*stairs_P_shift_amplitude;
ep[11]=stairs_A_Offset+stairs_A_shift_amplitude;
ram_data[DARWIN_STAIRS_CNTRL]|=RISE_LEFT_FOOT;
}
......@@ -417,13 +417,13 @@ void stairs_process(void)
ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0)+(stairs_Y_spread_amplitude/2.0)*delta;
ep[2]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height;
ep[3]=stairs_R_Offset+stairs_R_shift_amplitude+stairs_R_shift_amplitude*delta;
ep[4]=stairs_P_Offset-2.0*stairs_P_shift_amplitude+2.0*stairs_P_shift_amplitude*delta;
ep[4]=stairs_P_Offset-3.0*stairs_P_shift_amplitude+3.0*stairs_P_shift_amplitude*delta;
ep[5]=stairs_A_Offset+stairs_A_shift_amplitude-stairs_A_shift_amplitude*delta;
ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body-(stairs_X_shift_amplitude-stairs_X_shift_body)*delta;
ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0)-(stairs_Y_spread_amplitude/2.0)*delta;
ep[8]=stairs_Z_Offset;
ep[9]=stairs_R_Offset+stairs_R_shift_amplitude+stairs_R_shift_amplitude*delta;
ep[10]=stairs_P_Offset-2.0*stairs_P_shift_amplitude+2.0*stairs_P_shift_amplitude*delta;
ep[10]=stairs_P_Offset-3.0*stairs_P_shift_amplitude+3.0*stairs_P_shift_amplitude*delta;
ep[11]=stairs_A_Offset+stairs_A_shift_amplitude-stairs_A_shift_amplitude*delta;
ram_data[DARWIN_STAIRS_CNTRL]|=ADVANCE_LEFT_FOOT;
}
......@@ -482,33 +482,65 @@ void stairs_process(void)
// 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;
manager_current_slopes[R_HIP_YAW]=5;
}
if(manager_get_module(R_HIP_ROLL)==MM_WALKING)
{
manager_current_angles[R_HIP_ROLL]=((-180.0*angle[1])/PI)*65536.0;
manager_current_slopes[R_HIP_ROLL]=5;
}
if(manager_get_module(R_HIP_PITCH)==MM_WALKING)
{
manager_current_angles[R_HIP_PITCH]=((180.0*angle[2])/PI-stairs_Hip_Pitch_Offset)*65536.0;
manager_current_slopes[R_HIP_PITCH]=5;
}
if(manager_get_module(R_KNEE)==MM_WALKING)
{
manager_current_angles[R_KNEE]=((180.0*angle[3])/PI)*65536.0;
manager_current_slopes[R_KNEE]=5;
}
if(manager_get_module(R_ANKLE_PITCH)==MM_WALKING)
{
manager_current_angles[R_ANKLE_PITCH]=((-180.0*angle[4])/PI)*65536.0;
manager_current_slopes[R_ANKLE_PITCH]=5;
}
if(manager_get_module(R_ANKLE_ROLL)==MM_WALKING)
{
manager_current_angles[R_ANKLE_ROLL]=((180.0*angle[5])/PI)*65536.0;
manager_current_slopes[R_ANKLE_ROLL]=5;
}
if(manager_get_module(L_HIP_YAW)==MM_WALKING)
{
manager_current_angles[L_HIP_YAW]=((-180.0*angle[6])/PI)*65536.0;
manager_current_slopes[L_HIP_YAW]=5;
}
if(manager_get_module(L_HIP_ROLL)==MM_WALKING)
{
manager_current_angles[L_HIP_ROLL]=((-180.0*angle[7])/PI)*65536.0;
manager_current_slopes[L_HIP_ROLL]=5;
}
if(manager_get_module(L_HIP_PITCH)==MM_WALKING)
{
manager_current_angles[L_HIP_PITCH]=((-180.0*angle[8])/PI+stairs_Hip_Pitch_Offset)*65536.0;
manager_current_slopes[L_HIP_PITCH]=5;
}
if(manager_get_module(L_KNEE)==MM_WALKING)
{
manager_current_angles[L_KNEE]=((-180.0*angle[9])/PI)*65536.0;
manager_current_slopes[L_KNEE]=5;
}
if(manager_get_module(L_ANKLE_PITCH)==MM_WALKING)
{
manager_current_angles[L_ANKLE_PITCH]=((180.0*angle[10])/PI)*65536.0;
manager_current_slopes[L_ANKLE_PITCH]=5;
}
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;
manager_current_slopes[L_ANKLE_ROLL]=5;
}
}
}
......
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