Modified the stairs trajectory to avoid collisions with the stairs.

src/stairs.c

@@ -228,7 +228,7 @@ void stairs_process(void)
       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;
+      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[11]=stairs_A_Offset+stairs_A_shift_amplitude;
@@ -242,13 +242,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;
-      ep[4]=stairs_P_Offset-stairs_P_shift_amplitude+stairs_P_shift_amplitude*delta;
+      ep[4]=stairs_P_Offset-stairs_P_shift_amplitude;
       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;
+      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+stairs_P_shift_amplitude*delta;
+      ep[10]=stairs_P_Offset-stairs_P_shift_amplitude;
       ep[11]=stairs_A_Offset+stairs_A_shift_amplitude-stairs_A_shift_amplitude*delta;
       ram_data[DARWIN_STAIRS_CNTRL]|=ADVANCE_LEFT_FOOT;
     }
@@ -260,13 +260,13 @@ void stairs_process(void)
       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;
+      ep[4]=stairs_P_Offset-stairs_P_shift_amplitude+stairs_P_shift_amplitude*delta;
       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;
+      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;
+      ep[10]=stairs_P_Offset-stairs_P_shift_amplitude+stairs_P_shift_amplitude*delta;
       ep[11]=stairs_A_Offset;
       ram_data[DARWIN_STAIRS_CNTRL]|=CONTACT_LEFT_FOOT;
     }