diff --git a/include/darwin_registers.h b/include/darwin_registers.h
index 6cf97e222fddecd38a478dbd4298510851220d53..4a4684dc1914b1261ea7efe4eaa48a3c02c03cda 100644
--- a/include/darwin_registers.h
+++ b/include/darwin_registers.h
@@ -86,6 +86,24 @@ extern "C" {
#define GRIPPER_RIGHT_MIN_ANGLE ((unsigned short int)0x0060)
#define GRIPPER_RIGHT_MAX_FORCE ((unsigned short int)0x0062)
#define SMART_CHARGER_PERIOD ((unsigned short int)0x0064)
+#define STAIRS_PHASE1_TIME ((unsigned short int)0x0066)
+#define STAIRS_PHASE2_TIME ((unsigned short int)0x0068)
+#define STAIRS_PHASE3_TIME ((unsigned short int)0x006A)
+#define STAIRS_PHASE4_TIME ((unsigned short int)0x006C)
+#define STAIRS_PHASE5_TIME ((unsigned short int)0x006E)
+#define STAIRS_PHASE6_TIME ((unsigned short int)0x0070)
+#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_X_OFFSET ((unsigned short int)0x0078)
+#define STAIRS_Y_OFFSET ((unsigned short int)0x0079)
+#define STAIRS_Z_OFFSET ((unsigned short int)0x007A)
+#define STAIRS_Y_SHIFT ((unsigned short int)0x007B)
+#define STAIRS_X_LEFT_SHIFT ((unsigned short int)0x007C)
+#define STAIRS_X_RIGHT_SHIFT ((unsigned short int)0x007D)
+#define STAIRS_Z_OVERSHOOT ((unsigned short int)0x007E)
+#define STAIRS_Z_HEIGHT ((unsigned short int)0x007F)
+#define STAIRS_HIP_PITCH_OFF ((unsigned short int)0x0080)
#define LAST_EEPROM_OFFSET ((unsigned short int)0x00FF)
@@ -192,6 +210,34 @@ typedef enum {
DARWIN_GRIPPER_RIGHT_MAX_FORCE_H = GRIPPER_RIGHT_MAX_FORCE+1,
DARWIN_SMART_CHARGER_PERIOD_L = SMART_CHARGER_PERIOD, //en ms
DARWIN_SMART_CHARGER_PERIOD_H = SMART_CHARGER_PERIOD+1,
+ DARWIN_STAIRS_PHASE1_TIME_L = STAIRS_PHASE1_TIME,
+ DARWIN_STAIRS_PHASE1_TIME_H = STAIRS_PHASE1_TIME+1,
+ DARWIN_STAIRS_PHASE2_TIME_L = STAIRS_PHASE2_TIME,
+ DARWIN_STAIRS_PHASE2_TIME_H = STAIRS_PHASE2_TIME+1,
+ DARWIN_STAIRS_PHASE3_TIME_L = STAIRS_PHASE3_TIME,
+ DARWIN_STAIRS_PHASE3_TIME_H = STAIRS_PHASE3_TIME+1,
+ DARWIN_STAIRS_PHASE4_TIME_L = STAIRS_PHASE4_TIME,
+ DARWIN_STAIRS_PHASE4_TIME_H = STAIRS_PHASE4_TIME+1,
+ DARWIN_STAIRS_PHASE5_TIME_L = STAIRS_PHASE5_TIME,
+ DARWIN_STAIRS_PHASE5_TIME_H = STAIRS_PHASE5_TIME+1,
+ DARWIN_STAIRS_PHASE6_TIME_L = STAIRS_PHASE6_TIME,
+ DARWIN_STAIRS_PHASE6_TIME_H = STAIRS_PHASE6_TIME+1,
+ DARWIN_STAIRS_PHASE7_TIME_L = STAIRS_PHASE7_TIME,
+ DARWIN_STAIRS_PHASE7_TIME_H = STAIRS_PHASE7_TIME+1,
+ DARWIN_STAIRS_PHASE8_TIME_L = STAIRS_PHASE8_TIME,
+ 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_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_Z_OVERSHOOT = STAIRS_Z_OVERSHOOT,
+ DARWIN_STAIRS_Z_HEIGHT = STAIRS_Z_HEIGHT,
+ DARWIN_STAIRS_HIP_PITCH_OFF = STAIRS_HIP_PITCH_OFF,
+
//RAM
DARWIN_TX_LED_CNTRL = 0x0100, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
// | | | | blink | toggle | value | internally used
@@ -582,6 +628,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_registers;
#define GPIO_BASE_ADDRESS 0x0100
diff --git a/include/eeprom_init.h b/include/eeprom_init.h
index cb4543cfe461d97c1d856b49320e3b7af92296b1..570db02afd4ab008e6a766371ad177ebeea762c1 100644
--- a/include/eeprom_init.h
+++ b/include/eeprom_init.h
@@ -83,9 +83,25 @@ extern "C" {
#define DEFAULT_GRIPPER_RIGHT_MAX_ANGLE 0x0F00 // 30 in fixed point format 9|7
#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_SMART_CHARGER_PERIOD 0x05DC // 1500 ms
+#define DEFAULT_STAIRS_PHASE1_TIME 0x0064 // 100 ms
+#define DEFAULT_STAIRS_PHASE2_TIME 0x00C8 // 200 ms
+#define DEFAULT_STAIRS_PHASE3_TIME 0x012C // 300 ms
+#define DEFAULT_STAIRS_PHASE4_TIME 0x0190 // 400 ms
+#define DEFAULT_STAIRS_PHASE5_TIME 0x01F4 // 500 ms
+#define DEFAULT_STAIRS_PHASE6_TIME 0x0258 // 600 ms
+#define DEFAULT_STAIRS_PHASE7_TIME 0x02BC // 700 ms
+#define DEFAULT_STAIRS_PHASE8_TIME 0x0320 // 800 ms
+#define DEFAULT_STAIRS_PHASE9_TIME 0x0384 // 900 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_Y_SHIFT 0x0014 // 20 mm
+#define DEFAULT_STAIRS_X_LEFT_SHIFT 0x000A // 10 mm
+#define DEFAULT_STAIRS_X_RIGHT_SHIFT 0x0032 // 50 mm
+#define DEFAULT_STAIRS_Z_OVERSHOOT 0x000A // 10 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
#ifdef __cplusplus
}
diff --git a/include/stairs.h b/include/stairs.h
new file mode 100755
index 0000000000000000000000000000000000000000..6907e9f84876b91ea893becb4aee8d634029c03b
--- /dev/null
+++ b/include/stairs.h
@@ -0,0 +1,30 @@
+#ifndef _STAIRS_H
+#define _STAIRS_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "stm32f1xx.h"
+#include "motion_manager.h"
+
+// public functions
+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_stop(void);
+uint8_t is_climbing_stairs(void);
+
+// operation functions
+uint8_t stairs_in_range(unsigned short int address, unsigned short int length);
+void stairs_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
+void stairs_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
+// motion manager interface functions
+void stairs_process(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/eeprom.c b/src/eeprom.c
index fbb5a1a8caf6dd6e796739c795ad05376e2c41d6..b6eae9e83e835b5ff1e12a59a73c6c80467f28b8 100755
--- a/src/eeprom.c
+++ b/src/eeprom.c
@@ -156,7 +156,34 @@ uint16_t eeprom_data[] __attribute__ ((section (".eeprom")))={VALID_PAGE,
DEFAULT_GRIPPER_RIGHT_MAX_FORCE&0xFF, GRIPPER_RIGHT_MAX_FORCE,
DEFAULT_GRIPPER_RIGHT_MAX_FORCE>>8, GRIPPER_RIGHT_MAX_FORCE+1,
DEFAULT_SMART_CHARGER_PERIOD&0xFF, SMART_CHARGER_PERIOD,
- DEFAULT_SMART_CHARGER_PERIOD>>8, SMART_CHARGER_PERIOD+1};
+ DEFAULT_SMART_CHARGER_PERIOD>>8, SMART_CHARGER_PERIOD+1,
+ DEFAULT_STAIRS_PHASE1_TIME&0xFF, STAIRS_PHASE1_TIME,
+ DEFAULT_STAIRS_PHASE1_TIME>>8, STAIRS_PHASE1_TIME+1,
+ DEFAULT_STAIRS_PHASE2_TIME&0xFF, STAIRS_PHASE2_TIME,
+ DEFAULT_STAIRS_PHASE2_TIME>>8, STAIRS_PHASE2_TIME+1,
+ DEFAULT_STAIRS_PHASE3_TIME&0xFF, STAIRS_PHASE3_TIME,
+ DEFAULT_STAIRS_PHASE3_TIME>>8, STAIRS_PHASE3_TIME+1,
+ DEFAULT_STAIRS_PHASE4_TIME&0xFF, STAIRS_PHASE4_TIME,
+ DEFAULT_STAIRS_PHASE4_TIME>>8, STAIRS_PHASE4_TIME+1,
+ DEFAULT_STAIRS_PHASE5_TIME&0xFF, STAIRS_PHASE5_TIME,
+ DEFAULT_STAIRS_PHASE5_TIME>>8, STAIRS_PHASE5_TIME+1,
+ DEFAULT_STAIRS_PHASE6_TIME&0xFF, STAIRS_PHASE6_TIME,
+ DEFAULT_STAIRS_PHASE6_TIME>>8, STAIRS_PHASE6_TIME+1,
+ DEFAULT_STAIRS_PHASE7_TIME&0xFF, STAIRS_PHASE7_TIME,
+ DEFAULT_STAIRS_PHASE7_TIME>>8, STAIRS_PHASE7_TIME+1,
+ DEFAULT_STAIRS_PHASE8_TIME&0xFF, STAIRS_PHASE8_TIME,
+ 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_X_OFFSET, STAIRS_X_OFFSET,
+ DEFAULT_STAIRS_Y_OFFSET, STAIRS_Y_OFFSET,
+ DEFAULT_STAIRS_Z_OFFSET, STAIRS_Z_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_Z_OVERSHOOT, STAIRS_Z_OVERSHOOT,
+ DEFAULT_STAIRS_Z_HEIGHT, STAIRS_Z_HEIGHT,
+ DEFAULT_STAIRS_HIP_PITCH_OFF, STAIRS_HIP_PITCH_OFF};
/* Private function prototypes -----------------------------------------------*/
/* Private functions ---------------------------------------------------------*/
diff --git a/src/stairs.c b/src/stairs.c
new file mode 100755
index 0000000000000000000000000000000000000000..dfa470a0c52633f67eeb64c0107b3f26312f7cdf
--- /dev/null
+++ b/src/stairs.c
@@ -0,0 +1,339 @@
+#include "stairs.h"
+#include "darwin_kinematics.h"
+#include "darwin_math.h"
+#include "ram.h"
+#include <math.h>
+
+typedef enum {SHIFT_WEIGHT_LEFT,RISE_RIGHT_FOOT,ADVANCE_RIGHT_FOOT,CONTACT_RIGHT_FOOT,SHIFT_WEIGHT_RIGHT,RISE_LEFT_FOOT,ADVANCE_LEFT_FOOT,CONTACT_LEFT_FOOT,CENTER_WEIGHT} stairs_phase_t;
+
+// internal motion variables
+float m_Z_stair_height;
+float m_Z_overshoot;
+float m_Y_swap_amplitude;
+float m_X_left_swap_amplitude;
+float m_Y_right_swap_amplitude;
+
+// internal offset variables
+float m_Hip_Pitch_Offset;
+float m_X_Offset;
+float m_Y_Offset;
+float m_Z_Offset;
+
+// internal time variables
+float m_Time;
+float m_stairs_period;
+float m_shift_weight_left_time;
+float m_rise_right_foot_time;
+float advance_right_foot_time;
+float contact_right_foot_time;
+float shift_weight_right_time;
+float rise_left_foot_time;
+float advance_left_foot_time;
+float contact_left_foot_time;
+float center_weight_time;
+float total_time;
+
+// control variables
+uint8_t m_Ctrl_Running;
+
+// private functions
+
+// public functions
+void stairs_init(uint16_t period_us)
+{
+ // initialize the internal motion variables
+
+
+ // initialize internal control variables
+ m_Time=0;
+ stairs_period=((float)period_us)/1000.0;
+ m_Ctrl_Running=0x00;
+
+ stairs_process();
+}
+
+inline void stairs_set_period(uint16_t period_us)
+{
+ stairs_period=((float)period_us)/1000.0;
+}
+
+inline uint16_t stairs_get_period(void)
+{
+ return (uint16_t)(stairs_period*1000);
+}
+
+void stairs_start(void)
+{
+ ram_data[DARWIN_WALK_CNTRL]|=WALK_STATUS;
+ m_Ctrl_Running=0x01;
+}
+
+void stairs_stop(void)
+{
+ m_Ctrl_Running=0x00;
+}
+
+uint8_t is_climbing_stairs(void)
+{
+ if(ram_data[DARWIN_WALK_CNTRL]&WALK_STATUS)
+ return 0x01;
+ else
+ return 0x00;
+}
+
+// motion manager interface functions
+void stairs_process(void)
+{
+ float x_swap,y_swap,z_swap,a_swap,b_swap,c_swap;
+ float x_move_r,y_move_r,z_move_r,a_move_r,b_move_r,c_move_r;
+ float x_move_l,y_move_l,z_move_l,a_move_l,b_move_l,c_move_l;
+ float pelvis_offset_r,pelvis_offset_l;
+// float m_Body_Swing_Y,m_Body_Swing_Z;
+ float angle[14],ep[12];
+
+ if(m_Time==0)
+ {
+ update_param_time();
+ ram_data[DARWIN_WALK_CNTRL]&=(~WALK_PHASE);
+ ram_data[DARWIN_WALK_CNTRL]|=PHASE0;
+ if(m_Ctrl_Running==0x00)
+ {
+ if(m_X_Move_Amplitude==0 && m_Y_Move_Amplitude==0 && m_A_Move_Amplitude==0)
+ ram_data[DARWIN_WALK_CNTRL]&=(~WALK_STATUS);
+ else
+ {
+ ram_data[DARWIN_WALK_STEP_FW_BW]=0.0;
+ ram_data[DARWIN_WALK_STEP_LEFT_RIGHT]=0.0;
+ ram_data[DARWIN_WALK_STEP_DIRECTION]=0.0;
+ }
+ }
+ }
+ else if(m_Time>=(m_Phase_Time1-stairs_period/2.0) && m_Time<(m_Phase_Time1+stairs_period/2.0))
+ {
+ update_param_move();
+ ram_data[DARWIN_WALK_CNTRL]&=(~WALK_PHASE);
+ ram_data[DARWIN_WALK_CNTRL]|=PHASE1;
+ }
+ else if(m_Time>=(m_Phase_Time2-stairs_period/2.0) && m_Time<(m_Phase_Time2+stairs_period/2.0))
+ {
+ update_param_time();
+ m_Time=m_Phase_Time2;
+ ram_data[DARWIN_WALK_CNTRL]&=(~WALK_PHASE);
+ ram_data[DARWIN_WALK_CNTRL]|=PHASE2;
+ if(m_Ctrl_Running==0x00)
+ {
+ if(m_X_Move_Amplitude==0 && m_Y_Move_Amplitude==0 && m_A_Move_Amplitude==0)
+ ram_data[DARWIN_WALK_CNTRL]&=~WALK_STATUS;
+ else
+ {
+ ram_data[DARWIN_WALK_STEP_FW_BW]=0.0;
+ ram_data[DARWIN_WALK_STEP_LEFT_RIGHT]=0.0;
+ ram_data[DARWIN_WALK_STEP_DIRECTION]=0.0;
+ }
+ }
+ }
+ else if(m_Time>=(m_Phase_Time3-stairs_period/2.0) && m_Time<(m_Phase_Time3+stairs_period/2.0))
+ {
+ update_param_move();
+ ram_data[DARWIN_WALK_CNTRL]&=(~WALK_PHASE);
+ ram_data[DARWIN_WALK_CNTRL]|=PHASE3;
+ }
+ update_param_balance();
+
+ // Compute endpoints
+ x_swap=wsin(m_Time,m_X_Swap_PeriodTime,m_X_Swap_Phase_Shift,m_X_Swap_Amplitude,m_X_Swap_Amplitude_Shift);
+ y_swap=wsin(m_Time,m_Y_Swap_PeriodTime,m_Y_Swap_Phase_Shift,m_Y_Swap_Amplitude,m_Y_Swap_Amplitude_Shift);
+ z_swap=wsin(m_Time,m_Z_Swap_PeriodTime,m_Z_Swap_Phase_Shift,m_Z_Swap_Amplitude,m_Z_Swap_Amplitude_Shift);
+ a_swap=0;
+ b_swap=0;
+ c_swap=0;
+ if(m_Time<=m_SSP_Time_Start_L)
+ {
+ x_move_l=wsin(m_SSP_Time_Start_L,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_L,m_X_Move_Amplitude,m_X_Move_Amplitude_Shift);
+ y_move_l=wsin(m_SSP_Time_Start_L,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_L,m_Y_Move_Amplitude,m_Y_Move_Amplitude_Shift);
+ z_move_l=wsin(m_SSP_Time_Start_L,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_L,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift);
+ c_move_l=wsin(m_SSP_Time_Start_L,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_L,m_A_Move_Amplitude,m_A_Move_Amplitude_Shift);
+ x_move_r=wsin(m_SSP_Time_Start_L,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_L,-m_X_Move_Amplitude,-m_X_Move_Amplitude_Shift);
+ y_move_r=wsin(m_SSP_Time_Start_L,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_L,-m_Y_Move_Amplitude,-m_Y_Move_Amplitude_Shift);
+ z_move_r=wsin(m_SSP_Time_Start_R,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_R,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift);
+ c_move_r=wsin(m_SSP_Time_Start_L,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_L,-m_A_Move_Amplitude,-m_A_Move_Amplitude_Shift);
+ pelvis_offset_l=0;
+ pelvis_offset_r=0;
+ }
+ else if(m_Time<=m_SSP_Time_End_L)
+ {
+ x_move_l=wsin(m_Time,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_L,m_X_Move_Amplitude,m_X_Move_Amplitude_Shift);
+ y_move_l=wsin(m_Time,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_L,m_Y_Move_Amplitude,m_Y_Move_Amplitude_Shift);
+ z_move_l=wsin(m_Time,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_L,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift);
+ c_move_l=wsin(m_Time,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_L,m_A_Move_Amplitude,m_A_Move_Amplitude_Shift);
+ x_move_r=wsin(m_Time,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_L,-m_X_Move_Amplitude,-m_X_Move_Amplitude_Shift);
+ y_move_r=wsin(m_Time,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_L,-m_Y_Move_Amplitude,-m_Y_Move_Amplitude_Shift);
+ z_move_r=wsin(m_SSP_Time_Start_R,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_R,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift);
+ c_move_r=wsin(m_Time,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_L,-m_A_Move_Amplitude,-m_A_Move_Amplitude_Shift);
+ pelvis_offset_l=wsin(m_Time,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_L,m_Pelvis_Swing/2.0,m_Pelvis_Swing/2.0);
+ pelvis_offset_r=wsin(m_Time,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_L,-m_Pelvis_Offset/2.0,-m_Pelvis_Offset/2.0);
+ }
+ else if(m_Time<=m_SSP_Time_Start_R)
+ {
+ x_move_l=wsin(m_SSP_Time_End_L,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_L,m_X_Move_Amplitude,m_X_Move_Amplitude_Shift);
+ y_move_l=wsin(m_SSP_Time_End_L,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_L,m_Y_Move_Amplitude,m_Y_Move_Amplitude_Shift);
+ z_move_l=wsin(m_SSP_Time_End_L,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_L,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift);
+ c_move_l=wsin(m_SSP_Time_End_L,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_L,m_A_Move_Amplitude,m_A_Move_Amplitude_Shift);
+ x_move_r=wsin(m_SSP_Time_End_L,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_L,-m_X_Move_Amplitude,-m_X_Move_Amplitude_Shift);
+ y_move_r=wsin(m_SSP_Time_End_L,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_L,-m_Y_Move_Amplitude,-m_Y_Move_Amplitude_Shift);
+ z_move_r=wsin(m_SSP_Time_Start_R,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_R,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift);
+ c_move_r=wsin(m_SSP_Time_End_L,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_L,-m_A_Move_Amplitude,-m_A_Move_Amplitude_Shift);
+ pelvis_offset_l=0.0;
+ pelvis_offset_r=0.0;
+ }
+ else if(m_Time<=m_SSP_Time_End_R)
+ {
+ x_move_l=wsin(m_Time,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_R+PI,m_X_Move_Amplitude,m_X_Move_Amplitude_Shift);
+ y_move_l=wsin(m_Time,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_R+PI,m_Y_Move_Amplitude,m_Y_Move_Amplitude_Shift);
+ z_move_l=wsin(m_SSP_Time_End_L,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_L,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift);
+ c_move_l=wsin(m_Time,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_R+PI,m_A_Move_Amplitude,m_A_Move_Amplitude_Shift);
+ x_move_r=wsin(m_Time,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_R+PI,-m_X_Move_Amplitude,-m_X_Move_Amplitude_Shift);
+ y_move_r=wsin(m_Time,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_R+PI,-m_Y_Move_Amplitude,-m_Y_Move_Amplitude_Shift);
+ z_move_r=wsin(m_Time,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_R,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift);
+ c_move_r=wsin(m_Time,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_R+PI,-m_A_Move_Amplitude,-m_A_Move_Amplitude_Shift);
+ pelvis_offset_l=wsin(m_Time,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_R,m_Pelvis_Offset/2,m_Pelvis_Offset/2);
+ pelvis_offset_r=wsin(m_Time,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_R,-m_Pelvis_Swing/2,-m_Pelvis_Swing/2);
+ }
+ else
+ {
+ x_move_l=wsin(m_SSP_Time_End_R,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_R+PI,m_X_Move_Amplitude,m_X_Move_Amplitude_Shift);
+ y_move_l=wsin(m_SSP_Time_End_R,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_R+PI,m_Y_Move_Amplitude,m_Y_Move_Amplitude_Shift);
+ z_move_l=wsin(m_SSP_Time_End_L,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_L,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift);
+ c_move_l=wsin(m_SSP_Time_End_R,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_R+PI,m_A_Move_Amplitude,m_A_Move_Amplitude_Shift);
+ x_move_r=wsin(m_SSP_Time_End_R,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_R+PI,-m_X_Move_Amplitude,-m_X_Move_Amplitude_Shift);
+ y_move_r=wsin(m_SSP_Time_End_R,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_R+PI,-m_Y_Move_Amplitude,-m_Y_Move_Amplitude_Shift);
+ z_move_r=wsin(m_SSP_Time_End_R,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_R,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift);
+ c_move_r=wsin(m_SSP_Time_End_R,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_R+PI,-m_A_Move_Amplitude,-m_A_Move_Amplitude_Shift);
+ pelvis_offset_l=0.0;
+ pelvis_offset_r=0.0;
+ }
+ a_move_l=0.0;
+ b_move_l=0.0;
+ a_move_r=0.0;
+ b_move_r=0.0;
+
+ ep[0]=x_swap+x_move_r+m_X_Offset;
+ ep[1]=y_swap+y_move_r-m_Y_Offset / 2.0;
+ ep[2]=z_swap+z_move_r+m_Z_Offset;
+ ep[3]=a_swap+a_move_r-m_R_Offset / 2.0;
+ ep[4]=b_swap+b_move_r+m_P_Offset;
+ ep[5]=c_swap+c_move_r-m_A_Offset / 2.0;
+ ep[6]=x_swap+x_move_l+m_X_Offset;
+ ep[7]=y_swap+y_move_l+m_Y_Offset / 2.0;
+ ep[8]=z_swap+z_move_l+m_Z_Offset;
+ ep[9]=a_swap+a_move_l+m_R_Offset / 2.0;
+ ep[10]=b_swap+b_move_l+m_P_Offset;
+ ep[11]=c_swap+c_move_l+m_A_Offset / 2.0;
+
+ // Compute body swing
+/* if(m_Time<=m_SSP_Time_End_L)
+ {
+ m_Body_Swing_Y=-ep[7];
+ m_Body_Swing_Z=ep[8];
+ }
+ else
+ {
+ m_Body_Swing_Y=-ep[1];
+ m_Body_Swing_Z=ep[2];
+ }
+ m_Body_Swing_Z-=DARWIN_LEG_LENGTH;*/
+
+ // Compute arm swing
+ if(m_X_Move_Amplitude==0)
+ {
+ angle[12]=0; // Right
+ angle[13]=0; // Left
+ }
+ else
+ {
+ angle[12]=wsin(m_Time,m_PeriodTime,0.0,-m_X_Move_Amplitude*m_Arm_Swing_Gain,0.0);
+ angle[13]=wsin(m_Time,m_PeriodTime,0.0,m_X_Move_Amplitude*m_Arm_Swing_Gain,0.0);
+ }
+
+ if(ram_data[DARWIN_WALK_CNTRL]&WALK_STATUS)
+ {
+ m_Time += stairs_period;
+ if(m_Time >= m_PeriodTime)
+ m_Time = 0;
+ }
+
+ // 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))
+ return;// Do not use angles
+
+ // 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;
+ if(manager_get_module(R_HIP_ROLL)==MM_WALKING)
+ manager_current_angles[R_HIP_ROLL]=((-180.0*(angle[1]+pelvis_offset_r))/PI)*65536.0;
+ if(manager_get_module(R_HIP_PITCH)==MM_WALKING)
+ manager_current_angles[R_HIP_PITCH]=((180.0*angle[2])/PI-m_Hip_Pitch_Offset)*65536.0;
+ if(manager_get_module(R_KNEE)==MM_WALKING)
+ manager_current_angles[R_KNEE]=((180.0*angle[3])/PI)*65536.0;
+ if(manager_get_module(R_ANKLE_PITCH)==MM_WALKING)
+ manager_current_angles[R_ANKLE_PITCH]=((-180.0*angle[4])/PI)*65536.0;
+ if(manager_get_module(R_ANKLE_ROLL)==MM_WALKING)
+ manager_current_angles[R_ANKLE_ROLL]=((180.0*angle[5])/PI)*65536.0;
+ if(manager_get_module(L_HIP_YAW)==MM_WALKING)
+ manager_current_angles[L_HIP_YAW]=((-180.0*angle[6])/PI)*65536.0;
+ if(manager_get_module(L_HIP_ROLL)==MM_WALKING)
+ manager_current_angles[L_HIP_ROLL]=((-180.0*(angle[7]+pelvis_offset_l))/PI)*65536.0;
+ if(manager_get_module(L_HIP_PITCH)==MM_WALKING)
+ manager_current_angles[L_HIP_PITCH]=((-180.0*angle[8])/PI+m_Hip_Pitch_Offset)*65536.0;
+ if(manager_get_module(L_KNEE)==MM_WALKING)
+ manager_current_angles[L_KNEE]=((-180.0*angle[9])/PI)*65536.0;
+ if(manager_get_module(L_ANKLE_PITCH)==MM_WALKING)
+ manager_current_angles[L_ANKLE_PITCH]=((180.0*angle[10])/PI)*65536.0;
+ 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;
+}
+
+// operation functions
+uint8_t stairs_in_range(unsigned short int address, unsigned short int length)
+{
+ if(ram_in_window(WALK_BASE_ADDRESS,WALK_MEM_LENGTH,address,length) ||
+ ram_in_window(WALK_EEPROM_ADDRESS,WALK_EEPROM_LENGTH,address,length))
+ return 0x01;
+ else
+ return 0x00;
+}
+
+void stairs_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
+{
+
+}
+
+void stairs_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
+{
+ uint16_t i;
+
+ // walk control
+ if(ram_in_range(DARWIN_WALK_CNTRL,address,length))
+ {
+ if(data[DARWIN_WALK_CNTRL-address]&WALK_START)
+ stairs_start();
+ if(data[DARWIN_WALK_CNTRL-address]&WALK_STOP)
+ stairs_stop();
+ }
+ if(ram_in_range(DARWIN_WALK_STEP_FW_BW,address,length))
+ ram_data[DARWIN_WALK_STEP_FW_BW]=data[DARWIN_WALK_STEP_FW_BW-address];
+ if(ram_in_range(DARWIN_WALK_STEP_LEFT_RIGHT,address,length))
+ ram_data[DARWIN_WALK_STEP_LEFT_RIGHT]=data[DARWIN_WALK_STEP_LEFT_RIGHT-address];
+ if(ram_in_range(DARWIN_WALK_STEP_DIRECTION,address,length))
+ ram_data[DARWIN_WALK_STEP_DIRECTION]=data[DARWIN_WALK_STEP_DIRECTION-address];
+ // walk parameters
+ for(i=WALK_EEPROM_ADDRESS;i<=WALK_EEPROM_ADDRESS+WALK_EEPROM_LENGTH;i++)
+ if(ram_in_range(i,address,length))
+ ram_data[i]=data[i-address];
+}
+