diff --git a/src/stairs.c b/src/stairs.c index dd9668fcd5557b43ade6b0f2085400dbb41aa1da..22ac2e472caf9b030f66df1e6580989c3f61cdb0 100755 --- a/src/stairs.c +++ b/src/stairs.c @@ -2,7 +2,6 @@ #include "darwin_kinematics.h" #include "darwin_math.h" #include "ram.h" -#include <stdio.h> #include <math.h> typedef enum {SHIFT_WEIGHT_LEFT=0x10,RISE_RIGHT_FOOT=0x20,ADVANCE_RIGHT_FOOT=0x30,CONTACT_RIGHT_FOOT=0x40,SHIFT_WEIGHT_RIGHT=0x50,RISE_LEFT_FOOT=0x60,ADVANCE_LEFT_FOOT=0x70,CONTACT_LEFT_FOOT=0x80,CENTER_WEIGHT=0x90} stairs_phase_t; @@ -289,7 +288,6 @@ void stairs_process(void) ram_data[DARWIN_STAIRS_CNTRL]&=(~STAIRS_STATUS); stairs_Ctrl_Running=0x00; } - printf("%x,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n",ram_data[DARWIN_STAIRS_CNTRL]&STAIRS_PHASE,ep[0],ep[1],ep[2],ep[3],ep[4],ep[5],ep[6],ep[7],ep[8],ep[9],ep[10],ep[11]); if(ram_data[DARWIN_STAIRS_CNTRL]&STAIRS_STATUS) stairs_Time += stairs_period; @@ -299,10 +297,7 @@ void stairs_process(void) // 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)) - { - printf("Impossible to find inverse kinematics solution\n"); return;// Do not use angles - } // Compute motor value if(manager_get_module(R_HIP_YAW)==MM_WALKING)