From 52a03466d40b695745d92a38482b64906e1b0572 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Sergi=20Hern=C3=A0ndez=20Juan?= <shernand@iri.upc.edu> Date: Tue, 29 Aug 2017 20:58:15 +0200 Subject: [PATCH] Added the algorithm to go down stairs. Added the necessary changes in the dynamixel interface to execute it. --- include/darwin_registers.h | 9 +- include/stairs.h | 2 +- src/stairs.c | 510 +++++++++++++++++++++++++------------ 3 files changed, 350 insertions(+), 171 deletions(-) diff --git a/include/darwin_registers.h b/include/darwin_registers.h index 04c6658..cfa36fc 100644 --- a/include/darwin_registers.h +++ b/include/darwin_registers.h @@ -643,8 +643,8 @@ typedef enum { // | detected | enable DARWIN_GRIPPER_CNTRL = 0x0251, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 // left opened | right opened | left moving | right moving | close left | open left | close right | open right - DARWIN_STAIRS_CNTRL = 0x0252 // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 - // current phase | climbing | | stop stairs | start stairs + DARWIN_STAIRS_CNTRL = 0x0252 // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // current phase | climbing | stop stairs | start stairs down | start stairs up }darwin_registers; #define GPIO_BASE_ADDRESS 0x0100 @@ -753,8 +753,9 @@ typedef enum { #define STAIRS_MEM_LENGTH 1 #define STAIRS_EEPROM_ADDRESS 0x0066 #define STAIRS_EEPROM_LENGTH 30 -#define STAIRS_START 0x01 -#define STAIRS_STOP 0x02 +#define STAIRS_START_UP 0x01 +#define STAIRS_START_DOWN 0x02 +#define STAIRS_STOP 0x04 #define STAIRS_STATUS 0x08 #define STAIRS_PHASE 0xF0 diff --git a/include/stairs.h b/include/stairs.h index 08f4ba5..4cb0bac 100755 --- a/include/stairs.h +++ b/include/stairs.h @@ -12,7 +12,7 @@ extern "C" { void stairs_init(uint16_t period_us); inline void stairs_set_period(uint16_t period_us); inline uint16_t stairs_get_period(void); -void stairs_start(void); +void stairs_start(uint8_t up); void stairs_stop(void); uint8_t is_climbing_stairs(void); uint8_t stairs_get_phase(void); diff --git a/src/stairs.c b/src/stairs.c index 6a2eaf3..6ad7d3c 100755 --- a/src/stairs.c +++ b/src/stairs.c @@ -41,6 +41,7 @@ float stairs_center_weight_time; // control variables uint8_t stairs_Ctrl_Running; +uint8_t stairs_up; // private functions @@ -54,6 +55,7 @@ void stairs_init(uint16_t period_us) stairs_Time=0; stairs_period=((float)period_us)/1000.0; stairs_Ctrl_Running=0x00; + stairs_up=0x00; } inline void stairs_set_period(uint16_t period_us) @@ -66,7 +68,7 @@ inline uint16_t stairs_get_period(void) return (uint16_t)(stairs_period*1000); } -void stairs_start(void) +void stairs_start(uint8_t up) { // load all parameters stairs_shift_weight_left_time=((float)ram_data[DARWIN_STAIRS_PHASE1_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE1_TIME_H]<<8)); @@ -97,6 +99,7 @@ void stairs_start(void) // start operation ram_data[DARWIN_STAIRS_CNTRL]|=STAIRS_STATUS; stairs_Ctrl_Running=0x01; + stairs_up=up; } void stairs_stop(void) @@ -126,172 +129,345 @@ void stairs_process(void) if(ram_data[DARWIN_STAIRS_CNTRL]&STAIRS_STATUS) { ram_data[DARWIN_STAIRS_CNTRL]&=(~STAIRS_PHASE); - if(stairs_Time>=0 && stairs_Time<=stairs_shift_weight_left_time) + if(stairs_up) { - //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[2]=stairs_Z_Offset; - ep[3]=stairs_R_Offset; - ep[4]=stairs_P_Offset; - ep[5]=stairs_A_Offset; - ep[6]=stairs_X_Offset; - ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude*delta; - ep[8]=stairs_Z_Offset; - ep[9]=stairs_R_Offset; - ep[10]=stairs_P_Offset; - ep[11]=stairs_A_Offset; - ram_data[DARWIN_STAIRS_CNTRL]|=SHIFT_WEIGHT_LEFT; - } - else if(stairs_Time>stairs_shift_weight_left_time && stairs_Time<=stairs_rise_right_foot_time) - { - //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[2]=stairs_Z_Offset; - ep[3]=stairs_R_Offset-stairs_R_shift_amplitude*delta; - ep[4]=stairs_P_Offset; - 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_overshoot+stairs_Z_stair_height)*delta; - ep[9]=stairs_R_Offset-stairs_R_shift_amplitude*delta; - ep[10]=stairs_P_Offset; - ep[11]=stairs_A_Offset; - ram_data[DARWIN_STAIRS_CNTRL]|=RISE_RIGHT_FOOT; - } - else if(stairs_Time>stairs_rise_right_foot_time && stairs_Time<=stairs_advance_right_foot_time) - { - //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[2]=stairs_Z_Offset; - ep[3]=stairs_R_Offset-stairs_R_shift_amplitude; - ep[4]=stairs_P_Offset+stairs_P_shift_amplitude*delta; - ep[5]=stairs_A_Offset+stairs_A_shift_amplitude*delta; - ep[6]=stairs_X_Offset+stairs_X_shift_amplitude*delta; - ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0)*delta; - ep[8]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height; - ep[9]=stairs_R_Offset-stairs_R_shift_amplitude; - ep[10]=stairs_P_Offset+stairs_P_shift_amplitude*delta; - ep[11]=stairs_A_Offset+stairs_A_shift_amplitude*delta; - ram_data[DARWIN_STAIRS_CNTRL]|=ADVANCE_RIGHT_FOOT; - } - else if(stairs_Time>stairs_advance_right_foot_time && stairs_Time<=stairs_contact_right_foot_time) - { - //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[2]=stairs_Z_Offset; - ep[3]=stairs_R_Offset-stairs_R_shift_amplitude; - ep[4]=stairs_P_Offset+stairs_P_shift_amplitude; - ep[5]=stairs_A_Offset+stairs_A_shift_amplitude; - ep[6]=stairs_X_Offset+stairs_X_shift_amplitude; - 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_stair_height-stairs_Z_overshoot*delta; - ep[9]=stairs_R_Offset-stairs_R_shift_amplitude; - ep[10]=stairs_P_Offset+stairs_P_shift_amplitude; - ep[11]=stairs_A_Offset+stairs_A_shift_amplitude; - ram_data[DARWIN_STAIRS_CNTRL]|=CONTACT_RIGHT_FOOT; - } - else if(stairs_Time>stairs_contact_right_foot_time && stairs_Time<=stairs_shift_weight_right_time) - { - //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[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[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[11]=stairs_A_Offset+stairs_A_shift_amplitude; - ram_data[DARWIN_STAIRS_CNTRL]|=SHIFT_WEIGHT_RIGHT; - } - else if(stairs_Time>stairs_shift_weight_right_time && stairs_Time<=stairs_rise_left_foot_time) - { - //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[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[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[11]=stairs_A_Offset+stairs_A_shift_amplitude; - ram_data[DARWIN_STAIRS_CNTRL]|=RISE_LEFT_FOOT; - } - else if(stairs_Time>stairs_rise_left_foot_time && stairs_Time<=stairs_advance_left_foot_time) - { - //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[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[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[11]=stairs_A_Offset+stairs_A_shift_amplitude-stairs_A_shift_amplitude*delta; - ram_data[DARWIN_STAIRS_CNTRL]|=ADVANCE_LEFT_FOOT; - } - else if(stairs_Time>stairs_advance_left_foot_time && stairs_Time<=stairs_contact_left_foot_time) - { - //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[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; - 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[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; - ram_data[DARWIN_STAIRS_CNTRL]|=CONTACT_LEFT_FOOT; - } - else if(stairs_Time>stairs_contact_left_foot_time && stairs_Time<=stairs_center_weight_time) - { - //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[2]=stairs_Z_Offset+stairs_Z_stair_height-stairs_Z_stair_height*delta; - ep[3]=stairs_R_Offset; - ep[4]=stairs_P_Offset; - ep[5]=stairs_A_Offset; - ep[6]=stairs_X_Offset; - ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+stairs_Y_shift_amplitude*delta; - ep[8]=stairs_Z_Offset+stairs_Z_stair_height-stairs_Z_stair_height*delta; - ep[9]=stairs_R_Offset; - ep[10]=stairs_P_Offset; - ep[11]=stairs_A_Offset; - ram_data[DARWIN_STAIRS_CNTRL]|=CENTER_WEIGHT; + if(stairs_Time>=0 && stairs_Time<=stairs_shift_weight_left_time) + { + //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[2]=stairs_Z_Offset; + ep[3]=stairs_R_Offset; + ep[4]=stairs_P_Offset; + ep[5]=stairs_A_Offset; + ep[6]=stairs_X_Offset; + ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude*delta; + ep[8]=stairs_Z_Offset; + ep[9]=stairs_R_Offset; + ep[10]=stairs_P_Offset; + ep[11]=stairs_A_Offset; + ram_data[DARWIN_STAIRS_CNTRL]|=SHIFT_WEIGHT_LEFT; + } + else if(stairs_Time>stairs_shift_weight_left_time && stairs_Time<=stairs_rise_right_foot_time) + { + //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[2]=stairs_Z_Offset; + ep[3]=stairs_R_Offset-stairs_R_shift_amplitude*delta; + ep[4]=stairs_P_Offset; + 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_overshoot+stairs_Z_stair_height)*delta; + ep[9]=stairs_R_Offset-stairs_R_shift_amplitude*delta; + ep[10]=stairs_P_Offset; + ep[11]=stairs_A_Offset; + ram_data[DARWIN_STAIRS_CNTRL]|=RISE_RIGHT_FOOT; + } + else if(stairs_Time>stairs_rise_right_foot_time && stairs_Time<=stairs_advance_right_foot_time) + { + //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[2]=stairs_Z_Offset; + ep[3]=stairs_R_Offset-stairs_R_shift_amplitude; + ep[4]=stairs_P_Offset+stairs_P_shift_amplitude*delta; + ep[5]=stairs_A_Offset+stairs_A_shift_amplitude*delta; + ep[6]=stairs_X_Offset+stairs_X_shift_amplitude*delta; + ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0)*delta; + ep[8]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height; + ep[9]=stairs_R_Offset-stairs_R_shift_amplitude; + ep[10]=stairs_P_Offset+stairs_P_shift_amplitude*delta; + ep[11]=stairs_A_Offset+stairs_A_shift_amplitude*delta; + ram_data[DARWIN_STAIRS_CNTRL]|=ADVANCE_RIGHT_FOOT; + } + else if(stairs_Time>stairs_advance_right_foot_time && stairs_Time<=stairs_contact_right_foot_time) + { + //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[2]=stairs_Z_Offset; + ep[3]=stairs_R_Offset-stairs_R_shift_amplitude; + ep[4]=stairs_P_Offset+stairs_P_shift_amplitude; + ep[5]=stairs_A_Offset+stairs_A_shift_amplitude; + ep[6]=stairs_X_Offset+stairs_X_shift_amplitude; + 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_stair_height-stairs_Z_overshoot*delta; + ep[9]=stairs_R_Offset-stairs_R_shift_amplitude; + ep[10]=stairs_P_Offset+stairs_P_shift_amplitude; + ep[11]=stairs_A_Offset+stairs_A_shift_amplitude; + ram_data[DARWIN_STAIRS_CNTRL]|=CONTACT_RIGHT_FOOT; + } + else if(stairs_Time>stairs_contact_right_foot_time && stairs_Time<=stairs_shift_weight_right_time) + { + //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[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[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[11]=stairs_A_Offset+stairs_A_shift_amplitude; + ram_data[DARWIN_STAIRS_CNTRL]|=SHIFT_WEIGHT_RIGHT; + } + else if(stairs_Time>stairs_shift_weight_right_time && stairs_Time<=stairs_rise_left_foot_time) + { + //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[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[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[11]=stairs_A_Offset+stairs_A_shift_amplitude; + ram_data[DARWIN_STAIRS_CNTRL]|=RISE_LEFT_FOOT; + } + else if(stairs_Time>stairs_rise_left_foot_time && stairs_Time<=stairs_advance_left_foot_time) + { + //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[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[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[11]=stairs_A_Offset+stairs_A_shift_amplitude-stairs_A_shift_amplitude*delta; + ram_data[DARWIN_STAIRS_CNTRL]|=ADVANCE_LEFT_FOOT; + } + else if(stairs_Time>stairs_advance_left_foot_time && stairs_Time<=stairs_contact_left_foot_time) + { + //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[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; + 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[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; + ram_data[DARWIN_STAIRS_CNTRL]|=CONTACT_LEFT_FOOT; + } + else if(stairs_Time>stairs_contact_left_foot_time && stairs_Time<=stairs_center_weight_time) + { + //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[2]=stairs_Z_Offset+stairs_Z_stair_height-stairs_Z_stair_height*delta; + ep[3]=stairs_R_Offset; + ep[4]=stairs_P_Offset; + ep[5]=stairs_A_Offset; + ep[6]=stairs_X_Offset; + ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+stairs_Y_shift_amplitude*delta; + ep[8]=stairs_Z_Offset+stairs_Z_stair_height-stairs_Z_stair_height*delta; + ep[9]=stairs_R_Offset; + ep[10]=stairs_P_Offset; + ep[11]=stairs_A_Offset; + ram_data[DARWIN_STAIRS_CNTRL]|=CENTER_WEIGHT; + } + else + { + ram_data[DARWIN_STAIRS_CNTRL]&=(~STAIRS_STATUS); + stairs_Ctrl_Running=0x00; + } } else - { - ram_data[DARWIN_STAIRS_CNTRL]&=(~STAIRS_STATUS); - stairs_Ctrl_Running=0x00; + { + if(stairs_Time>=0 && stairs_Time<=stairs_shift_weight_left_time) + { + //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[2]=stairs_Z_Offset+stairs_Z_stair_height*delta; + ep[3]=stairs_R_Offset; + ep[4]=stairs_P_Offset; + ep[5]=stairs_A_Offset; + ep[6]=stairs_X_Offset; + ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude*delta; + ep[8]=stairs_Z_Offset+stairs_Z_stair_height*delta; + ep[9]=stairs_R_Offset; + ep[10]=stairs_P_Offset; + ep[11]=stairs_A_Offset; + ram_data[DARWIN_STAIRS_CNTRL]|=SHIFT_WEIGHT_LEFT; + } + else if(stairs_Time>stairs_shift_weight_left_time && stairs_Time<=stairs_rise_right_foot_time) + { + //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[2]=stairs_Z_Offset+stairs_Z_stair_height; + ep[3]=stairs_R_Offset-stairs_R_shift_amplitude; + ep[4]=stairs_P_Offset; + 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*delta; + ep[9]=stairs_R_Offset-stairs_R_shift_amplitude*delta; + ep[10]=stairs_P_Offset; + ep[11]=stairs_A_Offset; + ram_data[DARWIN_STAIRS_CNTRL]|=RISE_RIGHT_FOOT; + } + else if(stairs_Time>stairs_rise_right_foot_time && stairs_Time<=stairs_advance_right_foot_time) + { + //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[2]=stairs_Z_Offset+stairs_Z_stair_height; + ep[3]=stairs_R_Offset-stairs_R_shift_amplitude; + ep[4]=stairs_P_Offset+stairs_P_shift_amplitude*delta; + ep[5]=stairs_A_Offset+stairs_A_shift_amplitude*delta; + ep[6]=stairs_X_Offset+stairs_X_shift_amplitude*delta; + ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0)*delta; + ep[8]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height; + ep[9]=stairs_R_Offset-stairs_R_shift_amplitude; + ep[10]=stairs_P_Offset+stairs_P_shift_amplitude*delta; + ep[11]=stairs_A_Offset+stairs_A_shift_amplitude*delta; + ram_data[DARWIN_STAIRS_CNTRL]|=ADVANCE_RIGHT_FOOT; + } + else if(stairs_Time>stairs_advance_right_foot_time && stairs_Time<=stairs_contact_right_foot_time) + { + //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[2]=stairs_Z_Offset+stairs_Z_stair_height+stairs_Z_overshoot*delta; + ep[3]=stairs_R_Offset-stairs_R_shift_amplitude; + ep[4]=stairs_P_Offset+stairs_P_shift_amplitude; + ep[5]=stairs_A_Offset+stairs_A_shift_amplitude; + ep[6]=stairs_X_Offset+stairs_X_shift_amplitude; + 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_stair_height-stairs_Z_stair_height*delta; + ep[9]=stairs_R_Offset-stairs_R_shift_amplitude; + ep[10]=stairs_P_Offset+stairs_P_shift_amplitude; + ep[11]=stairs_A_Offset+stairs_A_shift_amplitude; + ram_data[DARWIN_STAIRS_CNTRL]|=CONTACT_RIGHT_FOOT; + } + else if(stairs_Time>stairs_contact_right_foot_time && stairs_Time<=stairs_shift_weight_right_time) + { + //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[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[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[11]=stairs_A_Offset+stairs_A_shift_amplitude; + ram_data[DARWIN_STAIRS_CNTRL]|=SHIFT_WEIGHT_RIGHT; + } + else if(stairs_Time>stairs_shift_weight_right_time && stairs_Time<=stairs_rise_left_foot_time) + { + //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[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[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[11]=stairs_A_Offset+stairs_A_shift_amplitude; + ram_data[DARWIN_STAIRS_CNTRL]|=RISE_LEFT_FOOT; + } + else if(stairs_Time>stairs_rise_left_foot_time && stairs_Time<=stairs_advance_left_foot_time) + { + //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[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[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[11]=stairs_A_Offset+stairs_A_shift_amplitude-stairs_A_shift_amplitude*delta; + ram_data[DARWIN_STAIRS_CNTRL]|=ADVANCE_LEFT_FOOT; + } + else if(stairs_Time>stairs_advance_left_foot_time && stairs_Time<=stairs_contact_left_foot_time) + { + //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[2]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height-(stairs_Z_stair_height+stairs_Z_overshoot)*delta; + ep[3]=stairs_R_Offset+2.0*stairs_R_shift_amplitude-stairs_R_shift_amplitude*delta; + ep[4]=stairs_P_Offset; + 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; + ep[9]=stairs_R_Offset+2.0*stairs_R_shift_amplitude-stairs_R_shift_amplitude*delta; + ep[10]=stairs_P_Offset; + ep[11]=stairs_A_Offset; + ram_data[DARWIN_STAIRS_CNTRL]|=CONTACT_LEFT_FOOT; + } + else if(stairs_Time>stairs_contact_left_foot_time && stairs_Time<=stairs_center_weight_time) + { + //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[2]=stairs_Z_Offset; + ep[3]=stairs_R_Offset+stairs_R_shift_amplitude-stairs_R_shift_amplitude*delta; + ep[4]=stairs_P_Offset; + ep[5]=stairs_A_Offset; + ep[6]=stairs_X_Offset; + ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+stairs_Y_shift_amplitude*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; + ep[11]=stairs_A_Offset; + ram_data[DARWIN_STAIRS_CNTRL]|=CENTER_WEIGHT; + } + else + { + ram_data[DARWIN_STAIRS_CNTRL]&=(~STAIRS_STATUS); + stairs_Ctrl_Running=0x00; + } } if(ram_data[DARWIN_STAIRS_CNTRL]&STAIRS_STATUS) @@ -358,8 +534,10 @@ void stairs_process_write_cmd(unsigned short int address,unsigned short int leng // walk control if(ram_in_range(DARWIN_STAIRS_CNTRL,address,length)) { - if(data[DARWIN_STAIRS_CNTRL-address]&STAIRS_START) - stairs_start(); + if(data[DARWIN_STAIRS_CNTRL-address]&STAIRS_START_UP) + stairs_start(0x01); + if(data[DARWIN_STAIRS_CNTRL-address]&STAIRS_START_DOWN) + stairs_start(0x00); if(data[DARWIN_STAIRS_CNTRL-address]&STAIRS_STOP) stairs_stop(); } -- GitLab