diff --git a/include/darwin_registers.h b/include/darwin_registers.h
index cfa36fc7a872651b1e0fdbf91d7f67722b9e7810..b84afcc7e78db9bde69d3b2fa67e03b84805ef40 100644
--- a/include/darwin_registers.h
+++ b/include/darwin_registers.h
@@ -752,7 +752,7 @@ typedef enum {
#define STAIRS_BASE_ADDRESS 0x0252
#define STAIRS_MEM_LENGTH 1
#define STAIRS_EEPROM_ADDRESS 0x0066
-#define STAIRS_EEPROM_LENGTH 30
+#define STAIRS_EEPROM_LENGTH 35
#define STAIRS_START_UP 0x01
#define STAIRS_START_DOWN 0x02
#define STAIRS_STOP 0x04
diff --git a/include/eeprom_init.h b/include/eeprom_init.h
index adf5729ca02dd901198f2d02467b89fbaadf2d29..09eca5c7ead93c546ad246a4cf536853dbc8d9af 100644
--- a/include/eeprom_init.h
+++ b/include/eeprom_init.h
@@ -104,8 +104,8 @@ extern "C" {
#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_R_SHIFT 0x000B // 2.875 degrees in fixed point format 5 (1+5) | 2
+#define DEFAULT_STAIRS_P_SHIFT 0x0017 // 5.73 degrees in fixed point format 5 (1+5) | 2
#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
diff --git a/src/stairs.c b/src/stairs.c
index 3947bd64cdf6c4b2dfc8c53280fc7069a076c6e1..68795d993ac479c7ea8754f35b6f40b2b1a4ccb0 100755
--- a/src/stairs.c
+++ b/src/stairs.c
@@ -85,8 +85,8 @@ void stairs_start(uint8_t up)
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_R_shift_amplitude=((float)((int8_t)ram_data[DARWIN_STAIRS_R_SHIFT]))*PI/720.0;// (r_shift/4)*(pi/180)
+ stairs_P_shift_amplitude=((float)((int8_t)ram_data[DARWIN_STAIRS_P_SHIFT]))*PI/720.0;// (p_shift/4)*(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]));
@@ -210,14 +210,14 @@ void stairs_process(void)
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-(3.0*stairs_P_shift_amplitude)*delta;
+ ep[3]=stairs_R_Offset-stairs_R_shift_amplitude+2.0*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_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-(3.0*stairs_P_shift_amplitude)*delta;
+ ep[9]=stairs_R_Offset-stairs_R_shift_amplitude+2.0*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;
}
@@ -228,13 +228,13 @@ void stairs_process(void)
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[3]=stairs_R_Offset+stairs_R_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[8]=stairs_Z_Offset+stairs_Z_stair_height;
+ ep[9]=stairs_R_Offset+stairs_R_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;
@@ -251,7 +251,7 @@ void stairs_process(void)
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[8]=stairs_Z_Offset+stairs_Z_stair_height;
ep[9]=stairs_R_Offset+stairs_R_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;
@@ -269,7 +269,7 @@ void stairs_process(void)
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[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+stairs_P_shift_amplitude*delta;
ep[11]=stairs_A_Offset;
@@ -380,14 +380,14 @@ void stairs_process(void)
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[3]=stairs_R_Offset-stairs_R_shift_amplitude+2.0*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_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[9]=stairs_R_Offset-stairs_R_shift_amplitude+2.0*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;
}
@@ -398,14 +398,14 @@ void stairs_process(void)
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[3]=stairs_R_Offset+stairs_R_shift_amplitude;
+ ep[4]=stairs_P_Offset-1.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[9]=stairs_R_Offset+stairs_R_shift_amplitude;
+ ep[10]=stairs_P_Offset-1.0*stairs_P_shift_amplitude;
ep[11]=stairs_A_Offset+stairs_A_shift_amplitude;
ram_data[DARWIN_STAIRS_CNTRL]|=RISE_LEFT_FOOT;
}
@@ -416,14 +416,14 @@ void stairs_process(void)
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[3]=stairs_R_Offset+stairs_R_shift_amplitude;//+stairs_R_shift_amplitude*delta;
+ ep[4]=stairs_P_Offset-1.0*stairs_P_shift_amplitude+1.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[9]=stairs_R_Offset+stairs_R_shift_amplitude;//+stairs_R_shift_amplitude*delta;
+ ep[10]=stairs_P_Offset-1.0*stairs_P_shift_amplitude+1.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;
}
@@ -434,13 +434,13 @@ void stairs_process(void)
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[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;
- ep[9]=stairs_R_Offset+2.0*stairs_R_shift_amplitude-stairs_R_shift_amplitude*delta;
+ 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;
@@ -482,33 +482,65 @@ void stairs_process(void)
// Compute motor value
if(manager_get_module(R_HIP_YAW)==MM_WALKING)
+ {
manager_current_angles[R_HIP_YAW]=((-180.0*angle[0])/PI)*65536.0;
+ manager_current_slopes[R_HIP_YAW]=5;
+ }
if(manager_get_module(R_HIP_ROLL)==MM_WALKING)
+ {
manager_current_angles[R_HIP_ROLL]=((-180.0*angle[1])/PI)*65536.0;
+ manager_current_slopes[R_HIP_ROLL]=5;
+ }
if(manager_get_module(R_HIP_PITCH)==MM_WALKING)
+ {
manager_current_angles[R_HIP_PITCH]=((180.0*angle[2])/PI-stairs_Hip_Pitch_Offset)*65536.0;
+ manager_current_slopes[R_HIP_PITCH]=5;
+ }
if(manager_get_module(R_KNEE)==MM_WALKING)
+ {
manager_current_angles[R_KNEE]=((180.0*angle[3])/PI)*65536.0;
+ manager_current_slopes[R_KNEE]=5;
+ }
if(manager_get_module(R_ANKLE_PITCH)==MM_WALKING)
+ {
manager_current_angles[R_ANKLE_PITCH]=((-180.0*angle[4])/PI)*65536.0;
+ manager_current_slopes[R_ANKLE_PITCH]=5;
+ }
if(manager_get_module(R_ANKLE_ROLL)==MM_WALKING)
+ {
manager_current_angles[R_ANKLE_ROLL]=((180.0*angle[5])/PI)*65536.0;
+ manager_current_slopes[R_ANKLE_ROLL]=5;
+ }
if(manager_get_module(L_HIP_YAW)==MM_WALKING)
+ {
manager_current_angles[L_HIP_YAW]=((-180.0*angle[6])/PI)*65536.0;
+ manager_current_slopes[L_HIP_YAW]=5;
+ }
if(manager_get_module(L_HIP_ROLL)==MM_WALKING)
+ {
manager_current_angles[L_HIP_ROLL]=((-180.0*angle[7])/PI)*65536.0;
+ manager_current_slopes[L_HIP_ROLL]=5;
+ }
if(manager_get_module(L_HIP_PITCH)==MM_WALKING)
+ {
manager_current_angles[L_HIP_PITCH]=((-180.0*angle[8])/PI+stairs_Hip_Pitch_Offset)*65536.0;
+ manager_current_slopes[L_HIP_PITCH]=5;
+ }
if(manager_get_module(L_KNEE)==MM_WALKING)
+ {
manager_current_angles[L_KNEE]=((-180.0*angle[9])/PI)*65536.0;
+ manager_current_slopes[L_KNEE]=5;
+ }
if(manager_get_module(L_ANKLE_PITCH)==MM_WALKING)
+ {
manager_current_angles[L_ANKLE_PITCH]=((180.0*angle[10])/PI)*65536.0;
+ manager_current_slopes[L_ANKLE_PITCH]=5;
+ }
if(manager_get_module(L_ANKLE_ROLL)==MM_WALKING)
+ {
manager_current_angles[L_ANKLE_ROLL]=((180.0*angle[11])/PI)*65536.0;
- if(manager_get_module(R_SHOULDER_PITCH)==MM_WALKING)
- manager_current_angles[R_SHOULDER_PITCH]=(angle[12]-48.345)*65536.0;
- if(manager_get_module(L_SHOULDER_PITCH)==MM_WALKING)
- manager_current_angles[L_SHOULDER_PITCH]=(-angle[13]+41.313)*65536.0;
+ manager_current_slopes[L_ANKLE_ROLL]=5;
+ }
}
}