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