Skip to content
Snippets Groups Projects
Commit fb1a3d88 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Modified the stairs up algorithm to make it more stable in the real robot.

parent 3e658674
No related branches found
No related tags found
2 merge requests!5Dynamixel manager,!2Smart charger fw
Hide whitespace changes
Inline Side-by-side
Showing
with 15 additions and 15 deletions
src/stairs.c
+ 15
− 15
View file @ fb1a3d88
......@@ -136,7 +136,7 @@ void stairs_process(void)
//1
delta=stairs_Time/stairs_shift_weight_left_time;
ep[0]=stairs_X_Offset;
ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude*delta;
ep[1]=-stairs_Y_Offset/2.0+stairs_Y_shift_amplitude*delta;
ep[2]=stairs_Z_Offset;
ep[3]=stairs_R_Offset;
ep[4]=stairs_P_Offset;
......@@ -154,7 +154,7 @@ void stairs_process(void)
//2
delta=(stairs_Time-stairs_shift_weight_left_time)/(stairs_rise_right_foot_time-stairs_shift_weight_left_time);
ep[0]=stairs_X_Offset;
ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude;
ep[1]=-stairs_Y_Offset/2.0+stairs_Y_shift_amplitude;
ep[2]=stairs_Z_Offset;
ep[3]=stairs_R_Offset-stairs_R_shift_amplitude*delta;
ep[4]=stairs_P_Offset;
......@@ -172,7 +172,7 @@ void stairs_process(void)
//3
delta=(stairs_Time-stairs_rise_right_foot_time)/(stairs_advance_right_foot_time-stairs_rise_right_foot_time);
ep[0]=stairs_X_Offset;
ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0)*delta;
ep[1]=-stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0)*delta;
ep[2]=stairs_Z_Offset;
ep[3]=stairs_R_Offset-stairs_R_shift_amplitude;
ep[4]=stairs_P_Offset+stairs_P_shift_amplitude*delta;
......@@ -190,7 +190,7 @@ void stairs_process(void)
//4
delta=(stairs_Time-stairs_advance_right_foot_time)/(stairs_contact_right_foot_time-stairs_advance_right_foot_time);
ep[0]=stairs_X_Offset;
ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0);
ep[1]=-stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0);
ep[2]=stairs_Z_Offset;
ep[3]=stairs_R_Offset-stairs_R_shift_amplitude;
ep[4]=stairs_P_Offset+stairs_P_shift_amplitude;
......@@ -208,16 +208,16 @@ void stairs_process(void)
//5
delta=(stairs_Time-stairs_contact_right_foot_time)/(stairs_shift_weight_right_time-stairs_contact_right_foot_time);
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[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-(2.0*stairs_P_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-(2.0*stairs_P_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;
}
......@@ -226,16 +226,16 @@ void stairs_process(void)
//6
delta=(stairs_Time-stairs_shift_weight_right_time)/(stairs_rise_left_foot_time-stairs_shift_weight_right_time);
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[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[4]=stairs_P_Offset-stairs_P_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[10]=stairs_P_Offset-stairs_P_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;
}
......@@ -244,16 +244,16 @@ void stairs_process(void)
//7
delta=(stairs_Time-stairs_rise_left_foot_time)/(stairs_advance_left_foot_time-stairs_rise_left_foot_time);
ep[0]=stairs_X_Offset-stairs_X_shift_body+stairs_X_shift_body*delta;
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[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;
ep[4]=stairs_P_Offset-stairs_P_shift_amplitude;
ep[4]=stairs_P_Offset-2.0*stairs_P_shift_amplitude+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+stairs_Z_stair_height-stairs_Z_overshoot;
ep[9]=stairs_R_Offset+stairs_R_shift_amplitude;
ep[10]=stairs_P_Offset-stairs_P_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;
ram_data[DARWIN_STAIRS_CNTRL]|=ADVANCE_LEFT_FOOT;
}
......@@ -262,7 +262,7 @@ void stairs_process(void)
//8
delta=(stairs_Time-stairs_advance_left_foot_time)/(stairs_contact_left_foot_time-stairs_advance_left_foot_time);
ep[0]=stairs_X_Offset;
ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude;
ep[1]=-stairs_Y_Offset/2.0-stairs_Y_shift_amplitude;
ep[2]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height-stairs_Z_overshoot*delta;
ep[3]=stairs_R_Offset+stairs_R_shift_amplitude-stairs_R_shift_amplitude*delta;
ep[4]=stairs_P_Offset-stairs_P_shift_amplitude+stairs_P_shift_amplitude*delta;
......@@ -280,7 +280,7 @@ void stairs_process(void)
//9
delta=(stairs_Time-stairs_contact_left_foot_time)/(stairs_center_weight_time-stairs_contact_left_foot_time);
ep[0]=stairs_X_Offset;
ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+stairs_Y_shift_amplitude*delta;
ep[1]=-stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+stairs_Y_shift_amplitude*delta;
ep[2]=stairs_Z_Offset+stairs_Z_stair_height-stairs_Z_stair_height*delta;
ep[3]=stairs_R_Offset;
ep[4]=stairs_P_Offset;
......
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