diff --git a/include/darwin_registers.h b/include/darwin_registers.h
index f12214f4e8c70e09beed55a23c557b3bfa42679a..04c66580089c71f7c0703ab1c12397b638095eb1 100644
--- a/include/darwin_registers.h
+++ b/include/darwin_registers.h
@@ -95,16 +95,22 @@ extern "C" {
#define STAIRS_PHASE7_TIME ((unsigned short int)0x0072)
#define STAIRS_PHASE8_TIME ((unsigned short int)0x0074)
#define STAIRS_PHASE9_TIME ((unsigned short int)0x0076)
-#define STAIRS_PHASE10_TIME ((unsigned short int)0x0078)
-#define STAIRS_X_OFFSET ((unsigned short int)0x007A)
-#define STAIRS_Y_OFFSET ((unsigned short int)0x007B)
-#define STAIRS_Z_OFFSET ((unsigned short int)0x007C)
-#define STAIRS_Y_SHIFT ((unsigned short int)0x007D)
-#define STAIRS_X_LEFT_SHIFT ((unsigned short int)0x007E)
-#define STAIRS_X_RIGHT_SHIFT ((unsigned short int)0x007F)
+#define STAIRS_X_OFFSET ((unsigned short int)0x0078)
+#define STAIRS_Y_OFFSET ((unsigned short int)0x0079)
+#define STAIRS_Z_OFFSET ((unsigned short int)0x007A)
+#define STAIRS_R_OFFSET ((unsigned short int)0x007B)
+#define STAIRS_P_OFFSET ((unsigned short int)0x007C)
+#define STAIRS_A_OFFSET ((unsigned short int)0x007D)
+#define STAIRS_Y_SHIFT ((unsigned short int)0x007E)
+#define STAIRS_X_SHIFT ((unsigned short int)0x007F)
#define STAIRS_Z_OVERSHOOT ((unsigned short int)0x0080)
#define STAIRS_Z_HEIGHT ((unsigned short int)0x0081)
#define STAIRS_HIP_PITCH_OFF ((unsigned short int)0x0082)
+#define STAIRS_R_SHIFT ((unsigned short int)0x0084)
+#define STAIRS_P_SHIFT ((unsigned short int)0x0085)
+#define STAIRS_A_SHIFT ((unsigned short int)0x0086)
+#define STAIRS_Y_SPREAD ((unsigned short int)0x0087)
+#define STAIRS_X_SHIFT_BODY ((unsigned short int)0x0088)
#define LAST_EEPROM_OFFSET ((unsigned short int)0x00FF)
@@ -229,18 +235,23 @@ typedef enum {
DARWIN_STAIRS_PHASE8_TIME_H = STAIRS_PHASE8_TIME+1,
DARWIN_STAIRS_PHASE9_TIME_L = STAIRS_PHASE9_TIME,
DARWIN_STAIRS_PHASE9_TIME_H = STAIRS_PHASE9_TIME+1,
- DARWIN_STAIRS_PHASE10_TIME_L = STAIRS_PHASE10_TIME,
- DARWIN_STAIRS_PHASE10_TIME_H = STAIRS_PHASE10_TIME+1,
DARWIN_STAIRS_X_OFFSET = STAIRS_X_OFFSET,
DARWIN_STAIRS_Y_OFFSET = STAIRS_Y_OFFSET,
DARWIN_STAIRS_Z_OFFSET = STAIRS_Z_OFFSET,
DARWIN_STAIRS_Y_SHIFT = STAIRS_Y_SHIFT,
- DARWIN_STAIRS_X_LEFT_SHIFT = STAIRS_X_LEFT_SHIFT,
- DARWIN_STAIRS_X_RIGHT_SHIFT = STAIRS_X_RIGHT_SHIFT,
+ DARWIN_STAIRS_R_OFFSET = STAIRS_R_OFFSET,
+ DARWIN_STAIRS_P_OFFSET = STAIRS_P_OFFSET,
+ DARWIN_STAIRS_A_OFFSET = STAIRS_A_OFFSET,
+ DARWIN_STAIRS_X_SHIFT = STAIRS_X_SHIFT,
DARWIN_STAIRS_Z_OVERSHOOT = STAIRS_Z_OVERSHOOT,
DARWIN_STAIRS_Z_HEIGHT = STAIRS_Z_HEIGHT,
DARWIN_STAIRS_HIP_PITCH_OFF_L = STAIRS_HIP_PITCH_OFF,
DARWIN_STAIRS_HIP_PITCH_OFF_H = STAIRS_HIP_PITCH_OFF+1,
+ DARWIN_STAIRS_R_SHIFT = STAIRS_R_SHIFT,
+ DARWIN_STAIRS_P_SHIFT = STAIRS_P_SHIFT,
+ DARWIN_STAIRS_A_SHIFT = STAIRS_A_SHIFT,
+ DARWIN_STAIRS_Y_SPREAD = STAIRS_Y_SPREAD,
+ DARWIN_STAIRS_X_SHIFT_BODY = STAIRS_X_SHIFT_BODY,
//RAM
DARWIN_TX_LED_CNTRL = 0x0100, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
diff --git a/include/eeprom.h b/include/eeprom.h
index c9c2f12b9cfe1aaa78eb37210b05ffa6c46138e5..5d227885a7c436bf53c08f10f51c986163b46e31 100755
--- a/include/eeprom.h
+++ b/include/eeprom.h
@@ -83,7 +83,7 @@ extern "C" {
#define PAGE_FULL ((uint8_t)0x80)
/* Variables' number */
-#define NB_OF_VAR ((uint8_t)0x83)
+#define NB_OF_VAR ((uint8_t)0x89)
/* Exported types ------------------------------------------------------------*/
/* Exported macro ------------------------------------------------------------*/
diff --git a/include/eeprom_init.h b/include/eeprom_init.h
index db1405a0269f0ccfe69559dc03fdbc55b292a19c..adf5729ca02dd901198f2d02467b89fbaadf2d29 100644
--- a/include/eeprom_init.h
+++ b/include/eeprom_init.h
@@ -84,27 +84,33 @@ extern "C" {
#define DEFAULT_GRIPPER_RIGHT_MIN_ANGLE 0xF100 // -30 in fixed point format 9|7
#define DEFAULT_GRIPPER_RIGHT_MAX_FORCE 0x0080 // 1023 max force in binary format
#define DEFAULT_SMART_CHARGER_PERIOD 0x05DC // 1500 ms
-#define DEFAULT_STAIRS_PHASE1_TIME 0x0640 // 100 ms
-#define DEFAULT_STAIRS_PHASE2_TIME 0x0C80 // 200 ms
-#define DEFAULT_STAIRS_PHASE3_TIME 0x12C0 // 300 ms
-#define DEFAULT_STAIRS_PHASE4_TIME 0x1900 // 400 ms
-#define DEFAULT_STAIRS_PHASE5_TIME 0x1F40 // 500 ms
-#define DEFAULT_STAIRS_PHASE6_TIME 0x2580 // 600 ms
-#define DEFAULT_STAIRS_PHASE7_TIME 0x2BC0 // 700 ms
-#define DEFAULT_STAIRS_PHASE8_TIME 0x3200 // 800 ms
-#define DEFAULT_STAIRS_PHASE9_TIME 0x3840 // 900 ms
-#define DEFAULT_STAIRS_PHASE10_TIME 0x3E80 // 1000 ms
+#define DEFAULT_STAIRS_PHASE1_TIME 0x0640 // 1600 ms
+#define DEFAULT_STAIRS_PHASE2_TIME 0x0C80 // 3200 ms
+#define DEFAULT_STAIRS_PHASE3_TIME 0x12C0 // 4800 ms
+#define DEFAULT_STAIRS_PHASE4_TIME 0x1900 // 6400 ms
+#define DEFAULT_STAIRS_PHASE5_TIME 0x1F40 // 8000 ms
+#define DEFAULT_STAIRS_PHASE6_TIME 0x2580 // 9600 ms
+#define DEFAULT_STAIRS_PHASE7_TIME 0x2BC0 // 11200 ms
+#define DEFAULT_STAIRS_PHASE8_TIME 0x3200 // 12800 ms
+#define DEFAULT_STAIRS_PHASE9_TIME 0x3840 // 14400 ms
#define DEFAULT_STAIRS_X_OFFSET 0xFFF6 // -10 mm
#define DEFAULT_STAIRS_Y_OFFSET 0x0005 // 5mm
#define DEFAULT_STAIRS_Z_OFFSET 0x0014 // 20 mm
+#define DEFAULT_STAIRS_R_OFFSET 0x0000 // 0 degrees in fixed point format 5 (1+4) | 3
+#define DEFAULT_STAIRS_P_OFFSET 0x0000 // 0 degrees in fixed point format 5 (1+4) | 3
+#define DEFAULT_STAIRS_A_OFFSET 0x0000 // 0 degrees in fixed point format 5 (1+4) | 3
#define DEFAULT_STAIRS_Y_SHIFT 0x0028 // 40 mm
-#define DEFAULT_STAIRS_X_LEFT_SHIFT 0x0000 // 0 mm
-#define DEFAULT_STAIRS_X_RIGHT_SHIFT 0x0050 // 80 mm
+#define DEFAULT_STAIRS_X_SHIFT 0x0050 // 80 mm
#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_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
-#define EEPROM_SIZE 131
+#define EEPROM_SIZE 137
#ifdef __cplusplus
}
diff --git a/src/eeprom.c b/src/eeprom.c
index 142ad7aa3f6ec9889424983cd563886eebd6beba..ad139dfcc25f9e27676bf89379891995bb91f38a 100755
--- a/src/eeprom.c
+++ b/src/eeprom.c
@@ -175,17 +175,23 @@ uint16_t eeprom_data[] __attribute__ ((section (".eeprom")))={VALID_PAGE,
DEFAULT_STAIRS_PHASE8_TIME>>8, STAIRS_PHASE8_TIME+1,
DEFAULT_STAIRS_PHASE9_TIME&0xFF, STAIRS_PHASE9_TIME,
DEFAULT_STAIRS_PHASE9_TIME>>8, STAIRS_PHASE9_TIME+1,
- DEFAULT_STAIRS_PHASE10_TIME&0xFF, STAIRS_PHASE10_TIME,
- DEFAULT_STAIRS_PHASE10_TIME>>8, STAIRS_PHASE10_TIME+1,
DEFAULT_STAIRS_X_OFFSET, STAIRS_X_OFFSET,
DEFAULT_STAIRS_Y_OFFSET, STAIRS_Y_OFFSET,
DEFAULT_STAIRS_Z_OFFSET, STAIRS_Z_OFFSET,
+ DEFAULT_STAIRS_R_OFFSET, STAIRS_R_OFFSET,
+ DEFAULT_STAIRS_P_OFFSET, STAIRS_P_OFFSET,
+ DEFAULT_STAIRS_A_OFFSET, STAIRS_A_OFFSET,
DEFAULT_STAIRS_Y_SHIFT, STAIRS_Y_SHIFT,
- DEFAULT_STAIRS_X_LEFT_SHIFT, STAIRS_X_LEFT_SHIFT,
- DEFAULT_STAIRS_X_RIGHT_SHIFT, STAIRS_X_RIGHT_SHIFT,
+ DEFAULT_STAIRS_X_SHIFT, STAIRS_X_SHIFT,
DEFAULT_STAIRS_Z_OVERSHOOT, STAIRS_Z_OVERSHOOT,
DEFAULT_STAIRS_Z_HEIGHT, STAIRS_Z_HEIGHT,
- DEFAULT_STAIRS_HIP_PITCH_OFF, STAIRS_HIP_PITCH_OFF};
+ DEFAULT_STAIRS_HIP_PITCH_OFF&0xFF, STAIRS_HIP_PITCH_OFF,
+ DEFAULT_STAIRS_HIP_PITCH_OFF>>8, STAIRS_HIP_PITCH_OFF+1,
+ DEFAULT_STAIRS_R_SHIFT, STAIRS_R_SHIFT,
+ DEFAULT_STAIRS_P_SHIFT, STAIRS_P_SHIFT,
+ DEFAULT_STAIRS_A_SHIFT, STAIRS_A_SHIFT,
+ DEFAULT_STAIRS_Y_SPREAD, STAIRS_Y_SPREAD,
+ DEFAULT_STAIRS_X_SHIFT_BODY, STAIRS_X_SHIFT_BODY};
/* Private function prototypes -----------------------------------------------*/
/* Private functions ---------------------------------------------------------*/
diff --git a/src/stairs.c b/src/stairs.c
index 5cefc8e0b9daf44f49ba4807216e0ffed2d7db49..dd9668fcd5557b43ade6b0f2085400dbb41aa1da 100755
--- a/src/stairs.c
+++ b/src/stairs.c
@@ -5,20 +5,27 @@
#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,STAND_UP=0xA0} stairs_phase_t;
+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;
// internal motion variables
float stairs_Z_stair_height;
float stairs_Z_overshoot;
float stairs_Y_shift_amplitude;
-float stairs_X_left_shift_amplitude;
-float stairs_X_right_shift_amplitude;
+float stairs_X_shift_amplitude;
+float stairs_R_shift_amplitude;
+float stairs_P_shift_amplitude;
+float stairs_A_shift_amplitude;
+float stairs_Y_spread_amplitude;
+float stairs_X_shift_body;
// internal offset variables
float stairs_Hip_Pitch_Offset;
float stairs_X_Offset;
float stairs_Y_Offset;
float stairs_Z_Offset;
+float stairs_R_Offset;
+float stairs_P_Offset;
+float stairs_A_Offset;
// internal time variables
float stairs_Time;
@@ -32,7 +39,6 @@ float stairs_rise_left_foot_time;
float stairs_advance_left_foot_time;
float stairs_contact_left_foot_time;
float stairs_center_weight_time;
-float stairs_walk_ready_time;
// control variables
uint8_t stairs_Ctrl_Running;
@@ -73,16 +79,22 @@ void stairs_start(void)
stairs_advance_left_foot_time=((float)ram_data[DARWIN_STAIRS_PHASE7_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE7_TIME_H]<<8));
stairs_contact_left_foot_time=((float)ram_data[DARWIN_STAIRS_PHASE8_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE8_TIME_H]<<8));
stairs_center_weight_time=((float)ram_data[DARWIN_STAIRS_PHASE9_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE9_TIME_H]<<8));
- stairs_walk_ready_time=((float)ram_data[DARWIN_STAIRS_PHASE10_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE10_TIME_H]<<8));
- stairs_Z_stair_height=((float)ram_data[DARWIN_STAIRS_Z_HEIGHT]);
- stairs_Z_overshoot=((float)ram_data[DARWIN_STAIRS_Z_OVERSHOOT]);
- stairs_Y_shift_amplitude=((float)ram_data[DARWIN_STAIRS_Y_SHIFT]);
- stairs_X_left_shift_amplitude=((float)ram_data[DARWIN_STAIRS_X_LEFT_SHIFT]);
- stairs_X_right_shift_amplitude=((float)ram_data[DARWIN_STAIRS_X_RIGHT_SHIFT]);
+ stairs_Z_stair_height=((float)((int8_t)ram_data[DARWIN_STAIRS_Z_HEIGHT]));
+ stairs_Z_overshoot=((float)((int8_t)ram_data[DARWIN_STAIRS_Z_OVERSHOOT]));
+ 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_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]));
stairs_X_Offset=((float)((int8_t)ram_data[DARWIN_STAIRS_X_OFFSET]));
stairs_Y_Offset=((float)((int8_t)ram_data[DARWIN_STAIRS_Y_OFFSET]));
stairs_Z_Offset=((float)((int8_t)ram_data[DARWIN_STAIRS_Z_OFFSET]));
+ stairs_R_Offset=((float)((int8_t)ram_data[DARWIN_STAIRS_X_OFFSET]))*PI/1440.0;
+ stairs_P_Offset=((float)((int8_t)ram_data[DARWIN_STAIRS_Y_OFFSET]))*PI/1440.0;
+ stairs_A_Offset=((float)((int8_t)ram_data[DARWIN_STAIRS_Z_OFFSET]))*PI/1440.0;
// start operation
ram_data[DARWIN_STAIRS_CNTRL]|=STAIRS_STATUS;
stairs_Ctrl_Running=0x01;
@@ -105,6 +117,7 @@ uint8_t is_climbing_stairs(void)
void stairs_process(void)
{
float angle[14]={0},ep[12]={0};
+ float delta;
if(ram_data[DARWIN_STAIRS_CNTRL]&STAIRS_STATUS)
{
@@ -112,156 +125,165 @@ void stairs_process(void)
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/stairs_shift_weight_left_time)*stairs_Time;
+ 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/stairs_shift_weight_left_time)*stairs_Time;
+ 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]=-0.05/(stairs_rise_right_foot_time-stairs_shift_weight_left_time)*(stairs_Time-stairs_shift_weight_left_time);
+ 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)/(stairs_rise_right_foot_time-stairs_shift_weight_left_time)*(stairs_Time-stairs_shift_weight_left_time);
- ep[9]=-0.05/(stairs_rise_right_foot_time-stairs_shift_weight_left_time)*(stairs_Time-stairs_shift_weight_left_time);
+ 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-10.0/(stairs_advance_right_foot_time-stairs_rise_right_foot_time)*(stairs_Time-stairs_rise_right_foot_time);
+ 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]=-0.05;
- ep[4]=0.1/(stairs_advance_right_foot_time-stairs_rise_right_foot_time)*(stairs_Time-stairs_rise_right_foot_time);
- ep[5]=0.3/(stairs_advance_right_foot_time-stairs_rise_right_foot_time)*(stairs_Time-stairs_rise_right_foot_time);
- ep[6]=stairs_X_Offset+stairs_X_right_shift_amplitude/(stairs_advance_right_foot_time-stairs_rise_right_foot_time)*(stairs_Time-stairs_rise_right_foot_time);
- ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+10.0/(stairs_advance_right_foot_time-stairs_rise_right_foot_time)*(stairs_Time-stairs_rise_right_foot_time);
+ 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]=-0.05;
- ep[10]=0.1/(stairs_advance_right_foot_time-stairs_rise_right_foot_time)*(stairs_Time-stairs_rise_right_foot_time);
- ep[11]=0.3/(stairs_advance_right_foot_time-stairs_rise_right_foot_time)*(stairs_Time-stairs_rise_right_foot_time);
+ 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-10.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]=-0.05;
- ep[4]=0.1;
- ep[5]=0.3;
- ep[6]=stairs_X_Offset+stairs_X_right_shift_amplitude;
- ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+10.0;
- ep[8]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height-stairs_Z_overshoot/(stairs_contact_right_foot_time-stairs_advance_right_foot_time)*(stairs_Time-stairs_advance_right_foot_time);
- ep[9]=-0.05;
- ep[10]=0.1;
- ep[11]=0.3;
+ 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
- ep[0]=stairs_X_Offset-35.0/(stairs_shift_weight_right_time-stairs_contact_right_foot_time)*(stairs_Time-stairs_contact_right_foot_time);
- ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-10.0-(2*stairs_Y_shift_amplitude)/(stairs_shift_weight_right_time-stairs_contact_right_foot_time)*(stairs_Time-stairs_contact_right_foot_time);
+ 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]=-0.05+0.05/(stairs_shift_weight_right_time-stairs_contact_right_foot_time)*(stairs_Time-stairs_contact_right_foot_time);
- ep[4]=0.1-0.2/(stairs_shift_weight_right_time-stairs_contact_right_foot_time)*(stairs_Time-stairs_contact_right_foot_time);
- ep[5]=0.3;
- ep[6]=stairs_X_Offset+stairs_X_right_shift_amplitude-35.0/(stairs_shift_weight_right_time-stairs_contact_right_foot_time)*(stairs_Time-stairs_contact_right_foot_time);
- ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+10.0-(2*stairs_Y_shift_amplitude)/(stairs_shift_weight_right_time-stairs_contact_right_foot_time)*(stairs_Time-stairs_contact_right_foot_time);
+ 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]=-0.05+0.05/(stairs_shift_weight_right_time-stairs_contact_right_foot_time)*(stairs_Time-stairs_contact_right_foot_time);
- ep[10]=0.1-0.2/(stairs_shift_weight_right_time-stairs_contact_right_foot_time)*(stairs_Time-stairs_contact_right_foot_time);
- ep[11]=0.3;
+ 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
- ep[0]=stairs_X_Offset-35.0;
- ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude-10.0;
- ep[2]=stairs_Z_Offset+(stairs_Z_overshoot+stairs_Z_stair_height)/(stairs_rise_left_foot_time-stairs_shift_weight_right_time)*(stairs_Time-stairs_shift_weight_right_time);
- ep[3]=0.05/(stairs_rise_left_foot_time-stairs_shift_weight_right_time)*(stairs_Time-stairs_shift_weight_right_time);
- ep[4]=-0.1;
- ep[5]=0.3;
- ep[6]=stairs_X_Offset+stairs_X_right_shift_amplitude-35.0;
- ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+10.0;
+ 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;
- ep[9]=0.05/(stairs_rise_left_foot_time-stairs_shift_weight_right_time)*(stairs_Time-stairs_shift_weight_right_time);
- ep[10]=-0.1;
- ep[11]=0.3;
+ 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
- ep[0]=stairs_X_Offset-35.0+35.0/(stairs_advance_left_foot_time-stairs_rise_left_foot_time)*(stairs_Time-stairs_rise_left_foot_time);
- ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude-10.0+10.0/(stairs_advance_left_foot_time-stairs_rise_left_foot_time)*(stairs_Time-stairs_rise_left_foot_time);
+ 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]=0.05;
- ep[4]=-0.1+0.1/(stairs_advance_left_foot_time-stairs_rise_left_foot_time)*(stairs_Time-stairs_rise_left_foot_time);
- ep[5]=0.3-0.3/(stairs_advance_left_foot_time-stairs_rise_left_foot_time)*(stairs_Time-stairs_rise_left_foot_time);
- ep[6]=stairs_X_Offset+stairs_X_right_shift_amplitude-35.0-(-35.0+stairs_X_right_shift_amplitude)/(stairs_advance_left_foot_time-stairs_rise_left_foot_time)*(stairs_Time-stairs_rise_left_foot_time);
- ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+10.0-10.0/(stairs_advance_left_foot_time-stairs_rise_left_foot_time)*(stairs_Time-stairs_rise_left_foot_time);
+ ep[3]=stairs_R_Offset+stairs_R_shift_amplitude;
+ ep[4]=stairs_P_Offset-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;
- ep[9]=0.05;
- ep[10]=-0.1+0.1/(stairs_advance_left_foot_time-stairs_rise_left_foot_time)*(stairs_Time-stairs_rise_left_foot_time);
- ep[11]=0.3-0.3/(stairs_advance_left_foot_time-stairs_rise_left_foot_time)*(stairs_Time-stairs_rise_left_foot_time);
+ ep[9]=stairs_R_Offset+stairs_R_shift_amplitude;
+ ep[10]=stairs_P_Offset-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;
}
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/(stairs_contact_left_foot_time-stairs_advance_left_foot_time)*(stairs_Time-stairs_advance_left_foot_time);
- ep[3]=0.05-0.05/(stairs_contact_left_foot_time-stairs_advance_left_foot_time)*(stairs_Time-stairs_advance_left_foot_time);
- ep[4]=0.0;
- ep[5]=0.0;
+ 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[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[9]=0.05-0.05/(stairs_contact_left_foot_time-stairs_advance_left_foot_time)*(stairs_Time-stairs_advance_left_foot_time);
- ep[10]=0.0;
- ep[11]=0.0;
+ 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]|=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/(stairs_center_weight_time-stairs_contact_left_foot_time)*(stairs_Time-stairs_contact_left_foot_time);
- ep[2]=stairs_Z_Offset+stairs_Z_stair_height-stairs_Z_stair_height/(stairs_center_weight_time-stairs_contact_left_foot_time)*(stairs_Time-stairs_contact_left_foot_time);
- ep[3]=0.0;
- ep[4]=0.0;
- ep[5]=0.0;
+ 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/(stairs_center_weight_time-stairs_contact_left_foot_time)*(stairs_Time-stairs_contact_left_foot_time);
- ep[8]=stairs_Z_Offset+stairs_Z_stair_height-stairs_Z_stair_height/(stairs_center_weight_time-stairs_contact_left_foot_time)*(stairs_Time-stairs_contact_left_foot_time);
- ep[9]=0.0;
- ep[10]=0.0;
- ep[11]=0.0;
+ 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 if(stairs_Time>stairs_center_weight_time && stairs_Time<=stairs_walk_ready_time)
- {
- ep[0]=stairs_X_Offset;
- ep[1]=stairs_Y_Offset/2.0;
- ep[2]=stairs_Z_Offset;
- ep[6]=stairs_X_Offset;
- ep[7]=stairs_Y_Offset/2.0;
- ep[8]=stairs_Z_Offset;
- ram_data[DARWIN_STAIRS_CNTRL]|=STAND_UP;
- }
else
{
ram_data[DARWIN_STAIRS_CNTRL]&=(~STAIRS_STATUS);