From 9a06538b4f79329dce51f3f4d48e41702a2180d5 Mon Sep 17 00:00:00 2001
From: Irene Garcia Camacho <igarcia@iri.upc.edu>
Date: Mon, 22 May 2017 16:33:38 +0200
Subject: [PATCH 01/21] Minor changes in some files related to the smart
 charger module

---
 include/darwin_registers.h | 78 ++++++++------------------------------
 include/dyn_battery.h      | 20 +++++-----
 include/smart_charger.h    |  4 +-
 src/cm730_fw.c             |  1 -
 src/motion_manager.c       | 34 ++++++++---------
 src/smart_charger.c        | 43 +++++++++++----------
 src/test_charger.c         | 27 ++++++++-----
 7 files changed, 83 insertions(+), 124 deletions(-)

diff --git a/include/darwin_registers.h b/include/darwin_registers.h
index 86ee43f..ae7cf2e 100644
--- a/include/darwin_registers.h
+++ b/include/darwin_registers.h
@@ -568,67 +568,19 @@ typedef enum {
   DARWIN_HEAD_MIN_TILT_H           = 0x0244,
   DARWIN_HEAD_TILT_TARGET_L        = 0x0245, // angle in fixed point format 9|7
   DARWIN_HEAD_TILT_TARGET_H        = 0x0246,
-   
-// to do: include more registers os smart charger's memory map to read
-/*  DARWIN_BATT_CHARGER_STATUS       = 0x0247,
-  DARWIN_BATT_INPUT_CURRENT_L      = 0x0248,
-  DARWIN_BATT_INPUT_CURRENT_H      = 0x0249,
-  DARWIN_BATT_CHARGE_CURRENT_L     = 0x024A,
-  DARWIN_BATT_CHARGE_CURRENT_H     = 0x024B,
-  DARWIN_BATT_CHARGE_VOLTAGE_L     = 0x024C,
-  DARWIN_BATT_CHARGE_VOLTAGE_H     = 0x024D,
-  DARWIN_BATT_LIMIT_CURRENT_L      = 0x024E,
-  DARWIN_BATT_LIMIT_CURRENT_H      = 0x024F, //
-  DARWIN_BATT_TEMPERATURE_L        = 0x0250,
-  DARWIN_BATT_TEMPERATURE_H        = 0x0251,
-  DARWIN_BATT_VOLTAGE_L            = 0x0252,
-  DARWIN_BATT_VOLTAGE_H            = 0x0253,
-  DARWIN_BATT_CURRENT_L            = 0x0254,
-  DARWIN_BATT_CURRENT_H            = 0x0255,
-  DARWIN_BATT_AVG_CURRENT_L        = 0x0256,
-  DARWIN_BATT_AVG_CURRENT_H        = 0x0257,
-  DARWIN_BATT_RELATIVE_SOC         = 0x0258,//
-  DARWIN_BATT_ABSOLUTE_SOC         = 0x0259,
-  DARWIN_BATT_REMAINING_CAP_L      = 0x025A,
-  DARWIN_BATT_REMAINING_CAP_H      = 0x025B,
-  DARWIN_BATT_FULL_CHARGE_CAP_L    = 0x025C,
-  DARWIN_BATT_FULL_CHARGE_CAP_H    = 0x025D,
-  DARWIN_BATT_RUN_TIME_EMPTY_L     = 0x025E, //
-  DARWIN_BATT_RUN_TIME_EMPTY_H     = 0x025F,
-  DARWIN_BATT_AVG_TIME_EMPTY_L     = 0x0260,
-  DARWIN_BATT_AVG_TIME_EMPTY_H     = 0x0261,
-  DARWIN_BATT_AVG_TIME_FULL_L      = 0x0262,
-  DARWIN_BATT_AVG_TIME_FULL_H      = 0x0263,
-  DARWIN_BATT_STATUS_L             = 0x0264,
-  DARWIN_BATT_STATUS_H             = 0x0265,
-  DARWIN_BATT_DESIGN_CAP_L         = 0x0266,
-  DARWIN_BATT_DESIGN_CAP_H         = 0x0267,
-  DARWIN_BATT_DESIGN_VOLTAGE_L     = 0x0268,
-  DARWIN_BATT_DESIGN_VOLTAGE_H     = 0x0269,
-  DARWIN_BATT_CELL1_VOLTAGE_L      = 0x026A,
-  DARWIN_BATT_CELL1_VOLTAGE_H      = 0x026B,
-  DARWIN_BATT_CELL2_VOLTAGE_L      = 0x026C,
-  DARWIN_BATT_CELL2_VOLTAGE_H      = 0x026D,
-  DARWIN_BATT_CELL3_VOLTAGE_L      = 0x026E,
-  DARWIN_BATT_CELL3_VOLTAGE_H      = 0x026F,
-  DARWIN_BATT_CELL4_VOLTAGE_L      = 0x0270,
-  DARWIN_BATT_CELL4_VOLTAGE_H      = 0x0271,
-*/  
-//añadidos 
+
   DARWIN_SMART_CHARGER_ID                = 0x0247,
-  DARWIN_SMART_CHARGER_STATUS            = 0x0248,
-  DARWIN_SMART_CHARGER_LIMIT_CURRENT_L   = 0x0249,
-  DARWIN_SMART_CHARGER_LIMIT_CURRENT_H   = 0x024A,
-  DARWIN_SMART_CHARGER_AVG_TIME_EMPTY_L  = 0x024B,
-  DARWIN_SMART_CHARGER_AVG_TIME_EMPTY_H  = 0x024C,
-  DARWIN_SMART_CHARGER_AVG_TIME_FULL_L   = 0x024D,
-  DARWIN_SMART_CHARGER_AVG_TIME_FULL_H   = 0x024E,
-  DARWIN_SMART_CHARGER_BATT_STATUS_L          = 0x024F,
-  DARWIN_SMART_CHARGER_BATT_STATUS_H          = 0x0250,
-  DARWIN_SMART_CHARGER_CNTRL             = 0x0251,  // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 |    bit 1   |   bit 0
-                                                    //                                               |  detected  |  enable    
-//Cambio registro
-  DARWIN_GRIPPER_CNTRL                   = 0x0252  //    bit 7    |     bit 6    | bit 5 | bit 4 |    bit 3   |    bit 2   |    bit 1    |   bit 0
+  DARWIN_SMART_CHARGER_LIMIT_CURRENT_L   = 0x0248, // Input max current in mA
+  DARWIN_SMART_CHARGER_LIMIT_CURRENT_H   = 0x0249,
+  DARWIN_SMART_CHARGER_AVG_TIME_EMPTY_L  = 0x024A, // Average time to empty batteries in min
+  DARWIN_SMART_CHARGER_AVG_TIME_EMPTY_H  = 0x024B,
+  DARWIN_SMART_CHARGER_AVG_TIME_FULL_L   = 0x024C, // Average time to full batteries in min
+  DARWIN_SMART_CHARGER_AVG_TIME_FULL_H   = 0x024D,
+  DARWIN_SMART_CHARGER_BATT_STATUS_L     = 0x024E, // Battery status
+  DARWIN_SMART_CHARGER_BATT_STATUS_H     = 0x024F,
+  DARWIN_SMART_CHARGER_CNTRL             = 0x0250, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 |    bit 1   |   bit 0
+                                                   //                                               |  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 |       |       | close left | open left  | close right | open right 
 }darwin_registers;
 
@@ -711,7 +663,7 @@ typedef enum {
 #define      HEAD_STOP               0x02
 #define      HEAD_STATUS             0x10
 
-#define      GRIPPER_BASE_ADDRESS    0x0252//0x0274
+#define      GRIPPER_BASE_ADDRESS    0x0251
 #define      GRIPPER_MEM_LENGTH      1
 #define      GRIPPER_EEPROM_ADDRESS  0x0054
 #define      GRIPPER_EEPROM_LENGTH   14
@@ -721,8 +673,8 @@ typedef enum {
 #define      GRIPPER_CLOSE_LEFT      0x08
 
 #define      SMART_CHARGER_BASE_ADDRESS    0x0247
-#define      SMART_CHARGER_MEM_LENGTH      10 //45
-#define      SMART_CHARGER_EEPROM_BASE     0x0060
+#define      SMART_CHARGER_MEM_LENGTH      10
+#define      SMART_CHARGER_EEPROM_BASE     0x0062
 #define      SMART_CHARGER_EEPROM_LENGTH   2
 #define      SMART_CHARGER_DET             0x01
 #define      SMART_CHARGER_EN              0x02
diff --git a/include/dyn_battery.h b/include/dyn_battery.h
index ac02ce8..70e47b6 100644
--- a/include/dyn_battery.h
+++ b/include/dyn_battery.h
@@ -45,7 +45,7 @@ typedef enum {
   BATTERY_OUTPUT_VOLTAGE_H        = 0x1E,
   /* RAM */
   BATTERY_CHARGER_STATUS          = 0x1F,
-  BATTERY_INPUT_CURRENT_L         = 0x20,//corriente consumida (mA)
+  BATTERY_INPUT_CURRENT_L         = 0x20,
   BATTERY_INPUT_CURRENT_H         = 0x21,
   BATTERY_CHARGE_CURRENT_L        = 0x22,
   BATTERY_CHARGE_CURRENT_H        = 0x23,
@@ -61,25 +61,25 @@ typedef enum {
   BATTERY_CURRENT_H               = 0x2D,
   BATTERY_AVG_CURRENT_L           = 0x2E,
   BATTERY_AVG_CURRENT_H           = 0x2F,
-  BATTERY_RELATIVE_SOC            = 0x30,//State Of Charge (%)
+  BATTERY_RELATIVE_SOC            = 0x30,
   BATTERY_ABSOLUTE_SOC            = 0x31,
-  BATTERY_REMAINING_CAP_L         = 0x32,//mAh (tiempo de vida)
+  BATTERY_REMAINING_CAP_L         = 0x32,
   BATTERY_REMAINING_CAP_H         = 0x33,
-  BATTERY_FULL_CHARGE_CAP_L       = 0x34,//
+  BATTERY_FULL_CHARGE_CAP_L       = 0x34,
   BATTERY_FULL_CHARGE_CAP_H       = 0x35,
-  BATTERY_RUN_TIME_EMPTY_L        = 0x36,//(min)
+  BATTERY_RUN_TIME_EMPTY_L        = 0x36,
   BATTERY_RUN_TIME_EMPTY_H        = 0x37,
   BATTERY_AVG_TIME_EMPTY_L        = 0x38,
-  BATTERY_AVG_TIME_EMPTY_H        = 0x39,//tiempo hasta que este descargada (min)
-  BATTERY_AVG_TIME_FULL_L         = 0x3A,//tiempo hasta que este cargada (min)
+  BATTERY_AVG_TIME_EMPTY_H        = 0x39,
+  BATTERY_AVG_TIME_FULL_L         = 0x3A,
   BATTERY_AVG_TIME_FULL_H         = 0x3B,
-  BATTERY_STATUS_L                = 0x3C,//cargando o descargando, cargada, descargada,...
+  BATTERY_STATUS_L                = 0x3C,
   BATTERY_STATUS_H                = 0x3D,
-  BATTERY_DESIGN_CAP_L            = 0x3E,//Capacidad de la bateria - numero constante
+  BATTERY_DESIGN_CAP_L            = 0x3E,
   BATTERY_DESIGN_CAP_H            = 0x3F,
   BATTERY_DESIGN_VOLTAGE_L        = 0x40,
   BATTERY_DESIGN_VOLTAGE_H        = 0x41,
-  BATTERY_CELL1_VOLTAGE_L         = 0x42,//Voltage de cada celda
+  BATTERY_CELL1_VOLTAGE_L         = 0x42,
   BATTERY_CELL1_VOLTAGE_H         = 0x43,
   BATTERY_CELL2_VOLTAGE_L         = 0x44,
   BATTERY_CELL2_VOLTAGE_H         = 0x45,
diff --git a/include/smart_charger.h b/include/smart_charger.h
index d6f8659..5d3587c 100644
--- a/include/smart_charger.h
+++ b/include/smart_charger.h
@@ -37,13 +37,13 @@ void smart_charger_init(uint16_t period_us);
  * 
  * \param master pointer to a TDynamixelMaster structure ((darwin_dyn_master or darwin_dyn_master_v2).
  */
-inline void smart_charger_set_version(TDynamixelMaster *master);
+inline void smart_charger_set_master(TDynamixelMaster *master);
 /**
  * \brief Function to get the master version of the Dynamixel protocol set to the module
 
  * \return the Dynamixel master structure associated to the smart charger module (darwin_dyn_master or darwin_dyn_master_v2).
  */
-inline TDynamixelMaster* smart_charger_get_version(void);
+inline TDynamixelMaster* smart_charger_get_master(void);
 /**
  * \brief Function to set smart charger's period
  * 
diff --git a/src/cm730_fw.c b/src/cm730_fw.c
index d074c76..5c1040f 100755
--- a/src/cm730_fw.c
+++ b/src/cm730_fw.c
@@ -14,7 +14,6 @@
 int main(void)
 {
   uint16_t eeprom_data,period;
-  uint8_t i;
 
   /* initialize the HAL module */
   HAL_Init();
diff --git a/src/motion_manager.c b/src/motion_manager.c
index 4672002..eeb4d17 100755
--- a/src/motion_manager.c
+++ b/src/motion_manager.c
@@ -322,20 +322,20 @@ void manager_init(uint16_t period_us)
   }
   
   //Other devices detected - smart charger
-  for(i=current; i<num; i++){
+  for(i=current; i<num; i++)
+  {
     dyn_master_read_word(&darwin_dyn_master,servo_ids[i],BATTERY_MODEL_NUMBER_L,&model);
     switch(model)
     {
       case DYN_BATTERY_MODEL: ram_data[DARWIN_SMART_CHARGER_CNTRL]=SMART_CHARGER_DET; //smart charger detected
-                              ram_data[DARWIN_SMART_CHARGER_ID] = servo_ids[i];        //smart charger ID
-                              smart_charger_set_version(&darwin_dyn_master);           //set bus Dynamixel master version 
-                              // Read smart charger's memory map
-                              //dyn_master_read_table(&darwin_dyn_master,ram_data[DARWIN_SMART_CHARGER_ID],BATTERY_CHARGER_STATUS,43,&ram_data[DARWIN_BATT_CHARGER_STATUS]);
-			      //Read charger status
-                              dyn_master_read_byte(&darwin_dyn_master,ram_data[DARWIN_SMART_CHARGER_ID],BATTERY_CHARGER_STATUS,&ram_data[DARWIN_SMART_CHARGER_STATUS]);
-			      break;
+                              ram_data[DARWIN_SMART_CHARGER_ID] = servo_ids[i];       //smart charger ID
+                              smart_charger_set_master(&darwin_dyn_master);          //set bus Dynamixel master version 
+                              // Set smart charger's memory map
+                              dyn_master_read_table(&darwin_dyn_master,ram_data[DARWIN_SMART_CHARGER_ID],BATTERY_AVG_TIME_EMPTY_L,6,&ram_data[DARWIN_SMART_CHARGER_AVG_TIME_EMPTY_L]);
+                              dyn_master_read_byte(&darwin_dyn_master,ram_data[DARWIN_SMART_CHARGER_ID],BATTERY_INPUT_MAX_CURRENT_L,&ram_data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_L]);
+                              break;
       default: break;
-      }
+    }
   }
     
   darwin_dyn_master_disable_power();
@@ -384,20 +384,20 @@ void manager_init(uint16_t period_us)
   }
   
   //Other devices detected - smart charger
-  for(i=current; i<num; i++){
+  for(i=current; i<num; i++)
+  {
     dyn_master_read_word(&darwin_dyn_master_v2,servo_ids[i],BATTERY_MODEL_NUMBER_L,&model);
     switch(model)
     {
       case DYN_BATTERY_MODEL: ram_data[DARWIN_SMART_CHARGER_CNTRL]=SMART_CHARGER_DET; //smart charger detected
-                              ram_data[DARWIN_SMART_CHARGER_ID] = servo_ids[i];        //smart charger ID
-                              smart_charger_set_version(&darwin_dyn_master_v2);        //Set bus Dynamixel master version 
-                              // Read smart charger's memory map
-                              //dyn_master_read_table(&darwin_dyn_master_v2,ram_data[DARWIN_SMART_CHARGER_ID],BATTERY_CHARGER_STATUS,43,&ram_data[DARWIN_BATT_CHARGER_STATUS]);         
-			      //Read charger status
-			      dyn_master_read_byte(&darwin_dyn_master_v2,ram_data[DARWIN_SMART_CHARGER_ID],BATTERY_CHARGER_STATUS,&ram_data[DARWIN_SMART_CHARGER_STATUS]);
+                              ram_data[DARWIN_SMART_CHARGER_ID] = servo_ids[i];       //smart charger ID
+                              smart_charger_set_master(&darwin_dyn_master_v2);       //Set bus Dynamixel master version 
+                              // Set smart charger's memory map
+                              dyn_master_read_table(&darwin_dyn_master_v2,ram_data[DARWIN_SMART_CHARGER_ID],BATTERY_AVG_TIME_EMPTY_L,6,&ram_data[DARWIN_SMART_CHARGER_AVG_TIME_EMPTY_L]);
+                              dyn_master_read_byte(&darwin_dyn_master_v2,ram_data[DARWIN_SMART_CHARGER_ID],BATTERY_INPUT_MAX_CURRENT_L,&ram_data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_L]);
                               break;
       default: break;
-     }
+    }
   }
   
   
diff --git a/src/smart_charger.c b/src/smart_charger.c
index 70de2e7..3812797 100644
--- a/src/smart_charger.c
+++ b/src/smart_charger.c
@@ -8,17 +8,17 @@
 uint8_t smart_charger_id;       //smart charger dynamixel ID
 uint8_t smart_charger_enabled;  //smart charger module enable signal
 uint8_t smart_charger_detected; //smart charger detect signal
-uint16_t smart_charger_period;  //smart charger period value
-uint16_t counter;               //for read access operations
+uint16_t smart_charger_period;  //smart charger period value in ms
+//uint16_t counter;               //for read access operations
 uint16_t smart_charger_count;   //counter = sc period / mm period --> Default: (1500ms*1000)/7800us
 uint8_t smart_charger_write;    //write smart charger signal
 uint8_t init_regs[6]={0xFF,0xFF,0xFF,0xFF,0xFF,0xFF};
 
-#define fifosize 32
+#define FIFOSIZE 32
 unsigned char *infifo;              //Pointer to fifo position where to store next input data
 unsigned char *outfifo;             //Pointer to fifo position where is stored the data to be written
 unsigned char numdata;              //Number of elements stored in the fifo
-unsigned char write_fifo[fifosize]; //Vector with data to write (data_write)
+unsigned char write_fifo[FIFOSIZE]; //Vector with data to write (data_write)
 unsigned char current[2];
 
 //---------------------------------------
@@ -29,11 +29,10 @@ TDynamixelMaster *dyn_battery_master;
 void smart_charger_init(uint16_t period_us)
 {
   smart_charger_id = ram_data[DARWIN_SMART_CHARGER_ID];
-  smart_charger_enabled = 0x00;
+  smart_charger_enabled = ram_data[DARWIN_SMART_CHARGER_CNTRL]&SMART_CHARGER_EN;
   smart_charger_detected = ram_data[DARWIN_SMART_CHARGER_CNTRL]&SMART_CHARGER_DET;
-  counter = 0;
-  smart_charger_period = ram_data[DARWIN_SMART_CHARGER_PERIOD_L];
-  smart_charger_period = (ram_data[DARWIN_SMART_CHARGER_PERIOD_H])<<8;
+ // counter = 0;
+  smart_charger_period = (ram_data[DARWIN_SMART_CHARGER_PERIOD_L]) + ((ram_data[DARWIN_SMART_CHARGER_PERIOD_H])<<8);
   smart_charger_count = (smart_charger_period*1000)/period_us;                                                                                
   smart_charger_write = 0x00;
   infifo=outfifo=&write_fifo[0];
@@ -41,20 +40,20 @@ void smart_charger_init(uint16_t period_us)
   ram_write_table(DARWIN_SMART_CHARGER_AVG_TIME_EMPTY_L, 6, init_regs);
 }
 
-inline void smart_charger_set_version(TDynamixelMaster *master)
+inline void smart_charger_set_master(TDynamixelMaster *master)
 {
   dyn_battery_master=master;
 }
 
-inline TDynamixelMaster* smart_charger_get_version(void)
+inline TDynamixelMaster* smart_charger_get_master(void)
 {
   return dyn_battery_master;
 }
 
-void smart_charger_set_period(uint16_t period)   //en ms
+void smart_charger_set_period(uint16_t period)
 {
-  uint16_t mm_period=manager_get_period_us();    //default: 7800us
-  smart_charger_count = (period*1000)/mm_period; //(ms*1000)/us
+  uint16_t mm_period=manager_get_period_us();
+  smart_charger_count = (period*1000)/mm_period;
   smart_charger_period = period;
   ram_data[DARWIN_SMART_CHARGER_PERIOD_L]=period&0x00FF;
   ram_data[DARWIN_SMART_CHARGER_PERIOD_H]=(period&0xFF00)>>8;
@@ -94,8 +93,8 @@ void smart_charger_process_read_cmd(unsigned short int address,unsigned short in
 
 void smart_charger_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
 {
-  //uint8_t i;
-  uint16_t period,i;
+  uint16_t period;//,i;
+  
 
   //Enable/Disable smart charger
   if(ram_in_range(DARWIN_SMART_CHARGER_CNTRL,address,length))
@@ -119,9 +118,9 @@ void smart_charger_process_write_cmd(unsigned short int address,unsigned short i
     current[1]=data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_H-address];
     
     
- /*   if(infifo==&write_fifo[fifosize]);
+ /*   if(infifo==&write_fifo[FIFOSIZE]);
       infifo=&write_fifo[0]; //go to first position of fifo
-    if(numdata<fifosize)  //free space in fifo
+    if(numdata<FIFOSIZE)  //free space in fifo
     { 
       for(i=DARWIN_SMART_CHARGER_LIMIT_CURRENT_L;i<=DARWIN_SMART_CHARGER_LIMIT_CURRENT_H;i++)
       {
@@ -144,17 +143,19 @@ void smart_charger_process_write_cmd(unsigned short int address,unsigned short i
 
 // motion manager interface function
 void smart_charger_process(void)
-{
-  gpio_set_led(LED_TX);//amarillo
-  counter++;
+{ 
   uint8_t error;
+  static uint16_t counter;
+  
+  counter++;
+
   //Write smart_charger - Battery limit current (EEPROM)
   if(smart_charger_detected && smart_charger_enabled && smart_charger_write && counter!=smart_charger_count)
   {
     error=dyn_master_write_table(dyn_battery_master,smart_charger_id,BATTERY_INPUT_MAX_CURRENT_L,2, current);
       if(error==DYN_SUCCESS){
 /*    dyn_master_write_table(dyn_battery_master,smart_charger_id,BATTERY_INPUT_MAX_CURRENT_L,2,outfifo);
-    if(outfifo==&write_fifo[fifosize-1])
+    if(outfifo==&write_fifo[FIFOSIZE-1])
       outfifo=&write_fifo[0]; //go to first position of fifo
     else 
       outfifo=outfifo+2; 
diff --git a/src/test_charger.c b/src/test_charger.c
index 5a547fc..23cb9b8 100644
--- a/src/test_charger.c
+++ b/src/test_charger.c
@@ -74,7 +74,7 @@ int main(void)
     gpio_set_led(LED_3);//Si existen dispositivos conectados -> luz verde
   }
 
-
+/*
 ////READ CHARGER STATUS
   unsigned short int address=DARWIN_SMART_CHARGER_STATUS;
   unsigned short int length=1;
@@ -84,9 +84,9 @@ int main(void)
   error=darwin_on_read(address,length,&data_read);
   if(data_read&0x01)
     gpio_set_led(LED_2); //verde
-    
+*/    
 
-/* ////READ PERIOD SMART CHARGER
+ ////READ PERIOD SMART CHARGER
   unsigned short int address=DARWIN_SMART_CHARGER_PERIOD_L;
   unsigned short int length=2;
   unsigned char data_read[2];
@@ -98,7 +98,7 @@ int main(void)
     gpio_set_led(LED_3); //verde
   else if(dread==0)
     gpio_set_led(LED_4); //azul
-*/
+
 
 /* ////READ ID SMART CHARGER
   unsigned char data_read2;
@@ -110,7 +110,7 @@ int main(void)
   //else if(data_read==0)
 */   
 
-/* ////WRITE PERIOD SMART CHARGER
+ ////WRITE PERIOD SMART CHARGER
   //HAL_Delay(500);
   unsigned short int address=DARWIN_SMART_CHARGER_PERIOD_L;
   unsigned short int length=2;
@@ -126,9 +126,9 @@ int main(void)
     gpio_set_led(LED_3); //verde
   else if(dread==1500) 
     gpio_set_led(LED_4); //azul
-*/
 
-/* ////ENABLE SMART CHARGER MODULE
+
+ ////ENABLE SMART CHARGER MODULE
   address=DARWIN_SMART_CHARGER_CNTRL;
   length=1;
   uint8_t data_read;
@@ -145,14 +145,21 @@ int main(void)
   darwin_on_read(address,length,&data_read);
   if(data_read==3) //Smart charger control = detected + enabled
     gpio_set_led(LED_3); //verde
-*/
+
     
-/* ////ENABLE MOTION MANAGER TIMER 
+ ////ENABLE MOTION MANAGER TIMER 
   address=DARWIN_MM_CNTRL;
   data_write=MANAGER_ENABLE;
   darwin_on_write(address,length,&data_write);
   HAL_Delay(500);
-*/
+
+  ////WRITE LIMIT CURRENT
+/*  address=DARWIN_SMART_CHARGER_LIMIT_CURRENT_L;
+  length=2;
+  data_write=0.512;
+  darwin_on_write(address,length,&data_write);
+  HAL_Delay(500);
+*/  
 
 /* //Read battery status
   address=DARWIN_BATT_AVG_TIME_EMPTY_L;
-- 
GitLab


From d57e7c1cee36b1e57a4ded44607bfb749dda842a Mon Sep 17 00:00:00 2001
From: Irene Garcia Camacho <igarcia@iri.upc.edu>
Date: Wed, 24 May 2017 11:12:03 +0200
Subject: [PATCH 02/21] Improved codign style, deleted comments.

---
 include/darwin_registers.h | 202 ++++++++++++++++++------------------
 include/eeprom_init.h      | 155 ++++++++++++++--------------
 src/eeprom.c               | 204 ++++++++++++++++++-------------------
 src/motion_manager.c       |   6 +-
 src/ram.c                  |   1 +
 src/smart_charger.c        |  60 ++---------
 6 files changed, 294 insertions(+), 334 deletions(-)

diff --git a/include/darwin_registers.h b/include/darwin_registers.h
index ae7cf2e..4ed2722 100644
--- a/include/darwin_registers.h
+++ b/include/darwin_registers.h
@@ -88,106 +88,106 @@ extern "C" {
 #define LAST_EEPROM_OFFSET              ((unsigned short int)0x00FF)
 
 typedef enum {
-  DARWIN_MODEL_NUMBER_L            = DEVICE_MODEL_OFFSET,
-  DARWIN_MODEL_NUMBER_H            = DEVICE_MODEL_OFFSET+1,
-  DARWIN_VERSION                   = FIRMWARE_VERSION_OFFSET,
-  DARWIN_ID                        = DEVICE_ID_OFFSET,
-  DARWIN_BAUD_RATE                 = BAUDRATE_OFFSET,
-  DARWIN_RETURN_DELAY_TIME         = RETURN_DELAY_OFFSET,
-  DARWIN_MM_PERIOD_L               = MM_PERIOD_OFFSET,
-  DARWIN_MM_PERIOD_H               = MM_PERIOD_OFFSET+1,
-  DARWIN_MM_BAL_KNEE_GAIN_L        = MM_BAL_KNEE_GAIN_OFFSET,// fixed point format 0|16
-  DARWIN_MM_BAL_KNEE_GAIN_H        = MM_BAL_KNEE_GAIN_OFFSET+1,
-  DARWIN_MM_BAL_ANKLE_ROLL_GAIN_L  = MM_BAL_ANKLE_ROLL_GAIN_OFFSET,// fixed point format 0|16
-  DARWIN_MM_BAL_ANKLE_ROLL_GAIN_H  = MM_BAL_ANKLE_ROLL_GAIN_OFFSET+1,
-  DARWIN_MM_BAL_ANKLE_PITCH_GAIN_L = MM_BAL_ANKLE_PITCH_GAIN_OFFSET,// fixed point format 0|16
-  DARWIN_MM_BAL_ANKLE_PITCH_GAIN_H = MM_BAL_ANKLE_PITCH_GAIN_OFFSET+1,
-  DARWIN_MM_BAL_HIP_ROLL_GAIN_L    = MM_BAL_HIP_ROLL_GAIN_OFFSET,// fixed point format 0|16
-  DARWIN_MM_BAL_HIP_ROLL_GAIN_H    = MM_BAL_HIP_ROLL_GAIN_OFFSET+1,
-  DARWIN_RETURN_LEVEL              = RETURN_LEVEL_OFFSET,
-  DARWIN_MM_SERVO0_OFFSET          = MM_SERVO0_OFFSET, // angle offset in fixed point format 1-3|4
-  DARWIN_MM_SERVO1_OFFSET          = MM_SERVO1_OFFSET, // angle offset in fixed point format 1-3|4
-  DARWIN_MM_SERVO2_OFFSET          = MM_SERVO2_OFFSET, // angle offset in fixed point format 1-3|4
-  DARWIN_MM_SERVO3_OFFSET          = MM_SERVO3_OFFSET, // angle offset in fixed point format 1-3|4
-  DARWIN_MM_SERVO4_OFFSET          = MM_SERVO4_OFFSET, // angle offset in fixed point format 1-3|4
-  DARWIN_MM_SERVO5_OFFSET          = MM_SERVO5_OFFSET, // angle offset in fixed point format 1-3|4
-  DARWIN_MM_SERVO6_OFFSET          = MM_SERVO6_OFFSET, // angle offset in fixed point format 1-3|4
-  DARWIN_MM_SERVO7_OFFSET          = MM_SERVO7_OFFSET, // angle offset in fixed point format 1-3|4
-  DARWIN_MM_SERVO8_OFFSET          = MM_SERVO8_OFFSET, // angle offset in fixed point format 1-3|4
-  DARWIN_MM_SERVO9_OFFSET          = MM_SERVO9_OFFSET, // angle offset in fixed point format 1-3|4
-  DARWIN_MM_SERVO10_OFFSET         = MM_SERVO10_OFFSET, // angle offset in fixed point format 1-3|4
-  DARWIN_MM_SERVO11_OFFSET         = MM_SERVO11_OFFSET, // angle offset in fixed point format 1-3|4
-  DARWIN_MM_SERVO12_OFFSET         = MM_SERVO12_OFFSET, // angle offset in fixed point format 1-3|4
-  DARWIN_MM_SERVO13_OFFSET         = MM_SERVO13_OFFSET, // angle offset in fixed point format 1-3|4
-  DARWIN_MM_SERVO14_OFFSET         = MM_SERVO14_OFFSET, // angle offset in fixed point format 1-3|4
-  DARWIN_MM_SERVO15_OFFSET         = MM_SERVO15_OFFSET, // angle offset in fixed point format 1-3|4
-  DARWIN_MM_SERVO16_OFFSET         = MM_SERVO16_OFFSET, // angle offset in fixed point format 1-3|4
-  DARWIN_MM_SERVO17_OFFSET         = MM_SERVO17_OFFSET, // angle offset in fixed point format 1-3|4
-  DARWIN_MM_SERVO18_OFFSET         = MM_SERVO18_OFFSET, // angle offset in fixed point format 1-3|4
-  DARWIN_MM_SERVO19_OFFSET         = MM_SERVO19_OFFSET, // angle offset in fixed point format 1-3|4
-  DARWIN_MM_SERVO20_OFFSET         = MM_SERVO20_OFFSET, // angle offset in fixed point format 1-3|4
-  DARWIN_MM_SERVO21_OFFSET         = MM_SERVO21_OFFSET, // angle offset in fixed point format 1-3|4
-  DARWIN_MM_SERVO22_OFFSET         = MM_SERVO22_OFFSET, // angle offset in fixed point format 1-3|4
-  DARWIN_MM_SERVO23_OFFSET         = MM_SERVO23_OFFSET, // angle offset in fixed point format 1-3|4
-  DARWIN_MM_SERVO24_OFFSET         = MM_SERVO24_OFFSET, // angle offset in fixed point format 1-3|4
-  DARWIN_MM_SERVO25_OFFSET         = MM_SERVO25_OFFSET, // angle offset in fixed point format 1-3|4
-  DARWIN_MM_SERVO26_OFFSET         = MM_SERVO26_OFFSET, // angle offset in fixed point format 1-3|4
-  DARWIN_MM_SERVO27_OFFSET         = MM_SERVO27_OFFSET, // angle offset in fixed point format 1-3|4
-  DARWIN_MM_SERVO28_OFFSET         = MM_SERVO28_OFFSET, // angle offset in fixed point format 1-3|4
-  DARWIN_MM_SERVO29_OFFSET         = MM_SERVO29_OFFSET, // angle offset in fixed point format 1-3|4
-  DARWIN_MM_SERVO30_OFFSET         = MM_SERVO30_OFFSET, // angle offset in fixed point format 1-3|4
-  DARWIN_MM_SERVO31_OFFSET         = MM_SERVO31_OFFSET, // angle offset in fixed point format 1-3|4
-  DARWIN_WALK_X_OFFSET             = WALK_X_OFFSET,
-  DARWIN_WALK_Y_OFFSET             = WALK_Y_OFFSET,
-  DARWIN_WALK_Z_OFFSET             = WALK_Z_OFFSET,
-  DARWIN_WALK_ROLL_OFFSET          = WALK_ROLL_OFFSET,
-  DARWIN_WALK_PITCH_OFFSET         = WALK_PITCH_OFFSET,
-  DARWIN_WALK_YAW_OFFSET           = WALK_YAW_OFFSET,
-  DARWIN_WALK_HIP_PITCH_OFF_L      = WALK_HIP_PITCH_OFF,
-  DARWIN_WALK_HIP_PITCH_OFF_H      = WALK_HIP_PITCH_OFF+1,
-  DARWIN_WALK_PERIOD_TIME_L        = WALK_PERIOD_TIME,
-  DARWIN_WALK_PERIOD_TIME_H        = WALK_PERIOD_TIME+1,
-  DARWIN_WALK_DSP_RATIO            = WALK_DSP_RATIO,
-  DARWIN_WALK_STEP_FW_BW_RATIO     = WALK_STEP_FW_BW_RATIO,
-  DARWIN_WALK_FOOT_HEIGHT          = WALK_FOOT_HEIGHT,
-  DARWIN_WALK_SWING_RIGHT_LEFT     = WALK_SWING_RIGHT_LEFT,
-  DARWIN_WALK_SWING_TOP_DOWN       = WALK_SWING_TOP_DOWN,
-  DARWIN_WALK_PELVIS_OFFSET        = WALK_PELVIS_OFFSET,
-  DARWIN_WALK_ARM_SWING_GAIN       = WALK_ARM_SWING_GAIN,
-  DARWIN_WALK_MAX_VEL              = WALK_MAX_VEL,
-  DARWIN_WALK_MAX_ROT_VEL          = WALK_MAX_ROT_VEL,
-  DARWIN_HEAD_PAN_P_L              = HEAD_PAN_P,// constant in fixed point format 0|16 
-  DARWIN_HEAD_PAN_P_H              = HEAD_PAN_P+1,
-  DARWIN_HEAD_PAN_I_L              = HEAD_PAN_I,// constant in fixed point format 0|16 
-  DARWIN_HEAD_PAN_I_H              = HEAD_PAN_I+1,
-  DARWIN_HEAD_PAN_D_L              = HEAD_PAN_D,// constant in fixed point format 0|16 
-  DARWIN_HEAD_PAN_D_H              = HEAD_PAN_D+1,
-  DARWIN_HEAD_PAN_I_CLAMP_L        = HEAD_PAN_I_CLAMP,// max error in fixed point format 9|7 
-  DARWIN_HEAD_PAN_I_CLAMP_H        = HEAD_PAN_I_CLAMP+1,
-  DARWIN_HEAD_TILT_P_L             = HEAD_TILT_P,// constant in fixed point format 0|16 
-  DARWIN_HEAD_TILT_P_H             = HEAD_TILT_P+1,
-  DARWIN_HEAD_TILT_I_L             = HEAD_TILT_I,// constant in fixed point format 0|16 
-  DARWIN_HEAD_TILT_I_H             = HEAD_TILT_I+1,
-  DARWIN_HEAD_TILT_D_L             = HEAD_TILT_D,// constant in fixed point format 0|16 
-  DARWIN_HEAD_TILT_D_H             = HEAD_TILT_D+1,
-  DARWIN_HEAD_TILT_I_CLAMP_L       = HEAD_TILT_I_CLAMP,// max error in fixed point format 9|7 
-  DARWIN_HEAD_TILT_I_CLAMP_H       = HEAD_TILT_I_CLAMP+1,
-  DARWIN_GRIPPER_LEFT_ID           = GRIPPER_LEFT_ID,
-  DARWIN_GRIPPER_LEFT_MAX_ANGLE_L  = GRIPPER_LEFT_MAX_ANGLE,
-  DARWIN_GRIPPER_LEFT_MAX_ANGLE_H  = GRIPPER_LEFT_MAX_ANGLE+1,
-  DARWIN_GRIPPER_LEFT_MIN_ANGLE_L  = GRIPPER_LEFT_MIN_ANGLE,
-  DARWIN_GRIPPER_LEFT_MIN_ANGLE_H  = GRIPPER_LEFT_MIN_ANGLE+1,
-  DARWIN_GRIPPER_LEFT_MAX_FORCE_L  = GRIPPER_LEFT_MAX_FORCE,
-  DARWIN_GRIPPER_LEFT_MAX_FORCE_H  = GRIPPER_LEFT_MAX_FORCE+1,
-  DARWIN_GRIPPER_RIGHT_ID          = GRIPPER_RIGHT_ID,
-  DARWIN_GRIPPER_RIGHT_MAX_ANGLE_L = GRIPPER_RIGHT_MAX_ANGLE,
-  DARWIN_GRIPPER_RIGHT_MAX_ANGLE_H = GRIPPER_RIGHT_MAX_ANGLE+1,
-  DARWIN_GRIPPER_RIGHT_MIN_ANGLE_L = GRIPPER_RIGHT_MIN_ANGLE,
-  DARWIN_GRIPPER_RIGHT_MIN_ANGLE_H = GRIPPER_RIGHT_MIN_ANGLE+1,
-  DARWIN_GRIPPER_RIGHT_MAX_FORCE_L = GRIPPER_RIGHT_MAX_FORCE,
-  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_MODEL_NUMBER_L                = DEVICE_MODEL_OFFSET,
+  DARWIN_MODEL_NUMBER_H                = DEVICE_MODEL_OFFSET+1,
+  DARWIN_VERSION                       = FIRMWARE_VERSION_OFFSET,
+  DARWIN_ID                            = DEVICE_ID_OFFSET,
+  DARWIN_BAUD_RATE                     = BAUDRATE_OFFSET,
+  DARWIN_RETURN_DELAY_TIME             = RETURN_DELAY_OFFSET,
+  DARWIN_MM_PERIOD_L                   = MM_PERIOD_OFFSET,
+  DARWIN_MM_PERIOD_H                   = MM_PERIOD_OFFSET+1,
+  DARWIN_MM_BAL_KNEE_GAIN_L            = MM_BAL_KNEE_GAIN_OFFSET,// fixed point format 0|16
+  DARWIN_MM_BAL_KNEE_GAIN_H            = MM_BAL_KNEE_GAIN_OFFSET+1,
+  DARWIN_MM_BAL_ANKLE_ROLL_GAIN_L      = MM_BAL_ANKLE_ROLL_GAIN_OFFSET,// fixed point format 0|16
+  DARWIN_MM_BAL_ANKLE_ROLL_GAIN_H      = MM_BAL_ANKLE_ROLL_GAIN_OFFSET+1,
+  DARWIN_MM_BAL_ANKLE_PITCH_GAIN_L     = MM_BAL_ANKLE_PITCH_GAIN_OFFSET,// fixed point format 0|16
+  DARWIN_MM_BAL_ANKLE_PITCH_GAIN_H     = MM_BAL_ANKLE_PITCH_GAIN_OFFSET+1,
+  DARWIN_MM_BAL_HIP_ROLL_GAIN_L        = MM_BAL_HIP_ROLL_GAIN_OFFSET,// fixed point format 0|16
+  DARWIN_MM_BAL_HIP_ROLL_GAIN_H        = MM_BAL_HIP_ROLL_GAIN_OFFSET+1,
+  DARWIN_RETURN_LEVEL                  = RETURN_LEVEL_OFFSET,
+  DARWIN_MM_SERVO0_OFFSET              = MM_SERVO0_OFFSET, // angle offset in fixed point format 1-3|4
+  DARWIN_MM_SERVO1_OFFSET              = MM_SERVO1_OFFSET, // angle offset in fixed point format 1-3|4
+  DARWIN_MM_SERVO2_OFFSET              = MM_SERVO2_OFFSET, // angle offset in fixed point format 1-3|4
+  DARWIN_MM_SERVO3_OFFSET              = MM_SERVO3_OFFSET, // angle offset in fixed point format 1-3|4
+  DARWIN_MM_SERVO4_OFFSET              = MM_SERVO4_OFFSET, // angle offset in fixed point format 1-3|4
+  DARWIN_MM_SERVO5_OFFSET              = MM_SERVO5_OFFSET, // angle offset in fixed point format 1-3|4
+  DARWIN_MM_SERVO6_OFFSET              = MM_SERVO6_OFFSET, // angle offset in fixed point format 1-3|4
+  DARWIN_MM_SERVO7_OFFSET              = MM_SERVO7_OFFSET, // angle offset in fixed point format 1-3|4
+  DARWIN_MM_SERVO8_OFFSET              = MM_SERVO8_OFFSET, // angle offset in fixed point format 1-3|4
+  DARWIN_MM_SERVO9_OFFSET              = MM_SERVO9_OFFSET, // angle offset in fixed point format 1-3|4
+  DARWIN_MM_SERVO10_OFFSET             = MM_SERVO10_OFFSET, // angle offset in fixed point format 1-3|4
+  DARWIN_MM_SERVO11_OFFSET             = MM_SERVO11_OFFSET, // angle offset in fixed point format 1-3|4
+  DARWIN_MM_SERVO12_OFFSET             = MM_SERVO12_OFFSET, // angle offset in fixed point format 1-3|4
+  DARWIN_MM_SERVO13_OFFSET             = MM_SERVO13_OFFSET, // angle offset in fixed point format 1-3|4
+  DARWIN_MM_SERVO14_OFFSET             = MM_SERVO14_OFFSET, // angle offset in fixed point format 1-3|4
+  DARWIN_MM_SERVO15_OFFSET             = MM_SERVO15_OFFSET, // angle offset in fixed point format 1-3|4
+  DARWIN_MM_SERVO16_OFFSET             = MM_SERVO16_OFFSET, // angle offset in fixed point format 1-3|4
+  DARWIN_MM_SERVO17_OFFSET             = MM_SERVO17_OFFSET, // angle offset in fixed point format 1-3|4
+  DARWIN_MM_SERVO18_OFFSET             = MM_SERVO18_OFFSET, // angle offset in fixed point format 1-3|4
+  DARWIN_MM_SERVO19_OFFSET             = MM_SERVO19_OFFSET, // angle offset in fixed point format 1-3|4
+  DARWIN_MM_SERVO20_OFFSET             = MM_SERVO20_OFFSET, // angle offset in fixed point format 1-3|4
+  DARWIN_MM_SERVO21_OFFSET             = MM_SERVO21_OFFSET, // angle offset in fixed point format 1-3|4
+  DARWIN_MM_SERVO22_OFFSET             = MM_SERVO22_OFFSET, // angle offset in fixed point format 1-3|4
+  DARWIN_MM_SERVO23_OFFSET             = MM_SERVO23_OFFSET, // angle offset in fixed point format 1-3|4
+  DARWIN_MM_SERVO24_OFFSET             = MM_SERVO24_OFFSET, // angle offset in fixed point format 1-3|4
+  DARWIN_MM_SERVO25_OFFSET             = MM_SERVO25_OFFSET, // angle offset in fixed point format 1-3|4
+  DARWIN_MM_SERVO26_OFFSET             = MM_SERVO26_OFFSET, // angle offset in fixed point format 1-3|4
+  DARWIN_MM_SERVO27_OFFSET             = MM_SERVO27_OFFSET, // angle offset in fixed point format 1-3|4
+  DARWIN_MM_SERVO28_OFFSET             = MM_SERVO28_OFFSET, // angle offset in fixed point format 1-3|4
+  DARWIN_MM_SERVO29_OFFSET             = MM_SERVO29_OFFSET, // angle offset in fixed point format 1-3|4
+  DARWIN_MM_SERVO30_OFFSET             = MM_SERVO30_OFFSET, // angle offset in fixed point format 1-3|4
+  DARWIN_MM_SERVO31_OFFSET             = MM_SERVO31_OFFSET, // angle offset in fixed point format 1-3|4
+  DARWIN_WALK_X_OFFSET                 = WALK_X_OFFSET,
+  DARWIN_WALK_Y_OFFSET                 = WALK_Y_OFFSET,
+  DARWIN_WALK_Z_OFFSET                 = WALK_Z_OFFSET,
+  DARWIN_WALK_ROLL_OFFSET              = WALK_ROLL_OFFSET,
+  DARWIN_WALK_PITCH_OFFSET             = WALK_PITCH_OFFSET,
+  DARWIN_WALK_YAW_OFFSET               = WALK_YAW_OFFSET,
+  DARWIN_WALK_HIP_PITCH_OFF_L          = WALK_HIP_PITCH_OFF,
+  DARWIN_WALK_HIP_PITCH_OFF_H          = WALK_HIP_PITCH_OFF+1,
+  DARWIN_WALK_PERIOD_TIME_L            = WALK_PERIOD_TIME,
+  DARWIN_WALK_PERIOD_TIME_H            = WALK_PERIOD_TIME+1,
+  DARWIN_WALK_DSP_RATIO                = WALK_DSP_RATIO,
+  DARWIN_WALK_STEP_FW_BW_RATIO         = WALK_STEP_FW_BW_RATIO,
+  DARWIN_WALK_FOOT_HEIGHT              = WALK_FOOT_HEIGHT,
+  DARWIN_WALK_SWING_RIGHT_LEFT         = WALK_SWING_RIGHT_LEFT,
+  DARWIN_WALK_SWING_TOP_DOWN           = WALK_SWING_TOP_DOWN,
+  DARWIN_WALK_PELVIS_OFFSET            = WALK_PELVIS_OFFSET,
+  DARWIN_WALK_ARM_SWING_GAIN           = WALK_ARM_SWING_GAIN,
+  DARWIN_WALK_MAX_VEL                  = WALK_MAX_VEL,
+  DARWIN_WALK_MAX_ROT_VEL              = WALK_MAX_ROT_VEL,
+  DARWIN_HEAD_PAN_P_L                  = HEAD_PAN_P,// constant in fixed point format 0|16 
+  DARWIN_HEAD_PAN_P_H                  = HEAD_PAN_P+1,
+  DARWIN_HEAD_PAN_I_L                  = HEAD_PAN_I,// constant in fixed point format 0|16 
+  DARWIN_HEAD_PAN_I_H                  = HEAD_PAN_I+1,
+  DARWIN_HEAD_PAN_D_L                  = HEAD_PAN_D,// constant in fixed point format 0|16 
+  DARWIN_HEAD_PAN_D_H                  = HEAD_PAN_D+1,
+  DARWIN_HEAD_PAN_I_CLAMP_L            = HEAD_PAN_I_CLAMP,// max error in fixed point format 9|7 
+  DARWIN_HEAD_PAN_I_CLAMP_H            = HEAD_PAN_I_CLAMP+1,
+  DARWIN_HEAD_TILT_P_L                 = HEAD_TILT_P,// constant in fixed point format 0|16 
+  DARWIN_HEAD_TILT_P_H                 = HEAD_TILT_P+1,
+  DARWIN_HEAD_TILT_I_L                 = HEAD_TILT_I,// constant in fixed point format 0|16 
+  DARWIN_HEAD_TILT_I_H                 = HEAD_TILT_I+1,
+  DARWIN_HEAD_TILT_D_L                 = HEAD_TILT_D,// constant in fixed point format 0|16 
+  DARWIN_HEAD_TILT_D_H                 = HEAD_TILT_D+1,
+  DARWIN_HEAD_TILT_I_CLAMP_L           = HEAD_TILT_I_CLAMP,// max error in fixed point format 9|7 
+  DARWIN_HEAD_TILT_I_CLAMP_H           = HEAD_TILT_I_CLAMP+1,
+  DARWIN_GRIPPER_LEFT_ID               = GRIPPER_LEFT_ID,
+  DARWIN_GRIPPER_LEFT_MAX_ANGLE_L      = GRIPPER_LEFT_MAX_ANGLE,
+  DARWIN_GRIPPER_LEFT_MAX_ANGLE_H      = GRIPPER_LEFT_MAX_ANGLE+1,
+  DARWIN_GRIPPER_LEFT_MIN_ANGLE_L      = GRIPPER_LEFT_MIN_ANGLE,
+  DARWIN_GRIPPER_LEFT_MIN_ANGLE_H      = GRIPPER_LEFT_MIN_ANGLE+1,
+  DARWIN_GRIPPER_LEFT_MAX_FORCE_L      = GRIPPER_LEFT_MAX_FORCE,
+  DARWIN_GRIPPER_LEFT_MAX_FORCE_H      = GRIPPER_LEFT_MAX_FORCE+1,
+  DARWIN_GRIPPER_RIGHT_ID              = GRIPPER_RIGHT_ID,
+  DARWIN_GRIPPER_RIGHT_MAX_ANGLE_L     = GRIPPER_RIGHT_MAX_ANGLE,
+  DARWIN_GRIPPER_RIGHT_MAX_ANGLE_H     = GRIPPER_RIGHT_MAX_ANGLE+1,
+  DARWIN_GRIPPER_RIGHT_MIN_ANGLE_L     = GRIPPER_RIGHT_MIN_ANGLE,
+  DARWIN_GRIPPER_RIGHT_MIN_ANGLE_H     = GRIPPER_RIGHT_MIN_ANGLE+1,
+  DARWIN_GRIPPER_RIGHT_MAX_FORCE_L     = GRIPPER_RIGHT_MAX_FORCE,
+  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,
 //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
@@ -673,7 +673,7 @@ typedef enum {
 #define      GRIPPER_CLOSE_LEFT      0x08
 
 #define      SMART_CHARGER_BASE_ADDRESS    0x0247
-#define      SMART_CHARGER_MEM_LENGTH      10
+#define      SMART_CHARGER_MEM_LENGTH      11
 #define      SMART_CHARGER_EEPROM_BASE     0x0062
 #define      SMART_CHARGER_EEPROM_LENGTH   2
 #define      SMART_CHARGER_DET             0x01
diff --git a/include/eeprom_init.h b/include/eeprom_init.h
index 7ba89ed..dafe223 100644
--- a/include/eeprom_init.h
+++ b/include/eeprom_init.h
@@ -5,84 +5,83 @@
 extern "C" {
 #endif
 
-#define    DEFAULT_DEVICE_MODEL             0x7300
-#define    DEFAULT_FIRMWARE_VERSION         0x0001
-#define    DEFAULT_DEVICE_ID                0x0002
-#define    DEFAULT_BAUDRATE                 0x0001
-#define    DEFAULT_RETURN_DELAY             0x0000
-#define    DEFAULT_MM_PERIOD                0x1E78 //7800us
-#define    DEFAULT_BAL_KNEE_GAIN            0x4CCD // 0.3 in fixed point format 0|16
-#define    DEFAULT_BAL_ANKLE_ROLL_GAIN      0xFFFF // 0.99999
-#define    DEFAULT_BAL_ANKLE_PITCH_GAIN     0xE666 // 0.9
-#define    DEFAULT_BAL_HIP_ROLL_GAIN        0x8000 // 0.5
-#define    DEFAULT_RETURN_LEVEL             0x0002
-#define    DEFAULT_SERVO0_OFFSET            0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO1_OFFSET            0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO2_OFFSET            0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO3_OFFSET            0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO4_OFFSET            0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO5_OFFSET            0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO6_OFFSET            0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO7_OFFSET            0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO8_OFFSET            0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO9_OFFSET            0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO10_OFFSET           0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO11_OFFSET           0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO12_OFFSET           0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO13_OFFSET           0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO14_OFFSET           0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO15_OFFSET           0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO16_OFFSET           0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO17_OFFSET           0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO18_OFFSET           0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO19_OFFSET           0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO20_OFFSET           0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO21_OFFSET           0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO22_OFFSET           0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO23_OFFSET           0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO24_OFFSET           0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO25_OFFSET           0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO26_OFFSET           0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO27_OFFSET           0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO28_OFFSET           0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO29_OFFSET           0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO30_OFFSET           0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO31_OFFSET           0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_WALK_X_OFFSET            0xFFF6 // -10 mm      
-#define    DEFAULT_WALK_Y_OFFSET            0x0005 // 5 mm
-#define    DEFAULT_WALK_Z_OFFSET            0x0014 // 20 mm
-#define    DEFAULT_WALK_ROLL_OFFSET         0x0000 // 0 degrees in fixed point format 5 (1+4) | 3
-#define    DEFAULT_WALK_PITCH_OFFSET        0x0000 // 0 degrees in fixed point format 5 (1+4) | 3
-#define    DEFAULT_WALK_YAW_OFFSET          0x0000 // 0 degrees in fixed point format 5 (1+4) | 3
-#define    DEFAULT_WALK_HIP_PITCH_OFF       0x34B6 // 13.18 degrees in fixed point format 6 (1+5) | 10 
-#define    DEFAULT_WALK_PERIOD_TIME         0x0258 // 600 ms
-#define    DEFAULT_WALK_DSP_RATIO           0x0019 // 0.1 in fixed point format 0 | 8
-#define    DEFAULT_WALK_STEP_FW_BW_RATIO    0x004C // 0.3 in fixed point format 0 | 8
-#define    DEFAULT_WALK_FOOT_HEIGHT         0x0028 // 40 mm
-#define    DEFAULT_WALK_SWING_RIGHT_LEFT    0x0014 // 20 mm
-#define    DEFAULT_WALK_SWING_TOP_DOWN      0x0005 // 5 mm
-#define    DEFAULT_WALK_PELVIS_OFFSET       0x0018 // 3 degrees in fixed point format 5 (1+4) | 3
-#define    DEFAULT_WALK_ARM_SWING_GAIN      0x0030 // 1.5 in fixed point format 3 | 5
-#define    DEFAULT_WALK_MAX_VEL             0x0016 // 20 mm/s
-#define    DEFAULT_WALK_MAX_ROT_VEL         0x0040 // 8 degrees/s in fixed point format 5 | 3
-#define    DEFAULT_HEAD_PAN_P               0x028F // 0.01 in fixed point format 0|16 
-#define    DEFAULT_HEAD_PAN_I               0x0000 // 0.0 in fixed point format 0|16
-#define    DEFAULT_HEAD_PAN_D               0x0000 // 0.0 in fixed point format 0|16
-#define    DEFAULT_HEAD_PAN_I_CLAMP         0x0000 // 0.0 in fixed point format 9|7
-#define    DEFAULT_HEAD_TILT_P              0x028F // 0.01 in fixed point format 0|16
-#define    DEFAULT_HEAD_TILT_I              0x0000 // 0.0 in fixed point format 0|16
-#define    DEFAULT_HEAD_TILT_D              0x0000 // 0.0 in fixed point format 0|16
-#define    DEFAULT_HEAD_TILT_I_CLAMP        0x0000 // 0.0 in fixed point format 9|7
-#define    DEFAULT_GRIPPER_LEFT_ID          0x0017 // ID 23 for the left gripper
-#define    DEFAULT_GRIPPER_LEFT_MAX_ANGLE   0x0F00 // 30 in fixed point format 9|7
-#define    DEFAULT_GRIPPER_LEFT_MIN_ANGLE   0xF100 // -30 in fixed point format 9|7
-#define    DEFAULT_GRIPPER_LEFT_MAX_FORCE   0x03FF // 1023 max force in binary format
-#define    DEFAULT_GRIPPER_RIGHT_ID         0x0018 // ID 24 for the left gripper
-#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  0x03FF // 1023 max force in binary format
- 
-#define    DEFAULT_SMART_CHARGER_PERIOD     0x05DC //1500 ms   (7,8ms*200) 
+#define    DEFAULT_DEVICE_MODEL                 0x7300
+#define    DEFAULT_FIRMWARE_VERSION             0x0001
+#define    DEFAULT_DEVICE_ID                    0x0002
+#define    DEFAULT_BAUDRATE                     0x0001
+#define    DEFAULT_RETURN_DELAY                 0x0000
+#define    DEFAULT_MM_PERIOD                    0x1E78 //7800us
+#define    DEFAULT_BAL_KNEE_GAIN                0x4CCD // 0.3 in fixed point format 0|16
+#define    DEFAULT_BAL_ANKLE_ROLL_GAIN          0xFFFF // 0.99999
+#define    DEFAULT_BAL_ANKLE_PITCH_GAIN         0xE666 // 0.9
+#define    DEFAULT_BAL_HIP_ROLL_GAIN            0x8000 // 0.5
+#define    DEFAULT_RETURN_LEVEL                 0x0002
+#define    DEFAULT_SERVO0_OFFSET                0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define    DEFAULT_SERVO1_OFFSET                0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define    DEFAULT_SERVO2_OFFSET                0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define    DEFAULT_SERVO3_OFFSET                0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define    DEFAULT_SERVO4_OFFSET                0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define    DEFAULT_SERVO5_OFFSET                0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define    DEFAULT_SERVO6_OFFSET                0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define    DEFAULT_SERVO7_OFFSET                0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define    DEFAULT_SERVO8_OFFSET                0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define    DEFAULT_SERVO9_OFFSET                0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define    DEFAULT_SERVO10_OFFSET               0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define    DEFAULT_SERVO11_OFFSET               0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define    DEFAULT_SERVO12_OFFSET               0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define    DEFAULT_SERVO13_OFFSET               0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define    DEFAULT_SERVO14_OFFSET               0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define    DEFAULT_SERVO15_OFFSET               0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define    DEFAULT_SERVO16_OFFSET               0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define    DEFAULT_SERVO17_OFFSET               0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define    DEFAULT_SERVO18_OFFSET               0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define    DEFAULT_SERVO19_OFFSET               0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define    DEFAULT_SERVO20_OFFSET               0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define    DEFAULT_SERVO21_OFFSET               0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define    DEFAULT_SERVO22_OFFSET               0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define    DEFAULT_SERVO23_OFFSET               0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define    DEFAULT_SERVO24_OFFSET               0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define    DEFAULT_SERVO25_OFFSET               0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define    DEFAULT_SERVO26_OFFSET               0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define    DEFAULT_SERVO27_OFFSET               0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define    DEFAULT_SERVO28_OFFSET               0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define    DEFAULT_SERVO29_OFFSET               0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define    DEFAULT_SERVO30_OFFSET               0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define    DEFAULT_SERVO31_OFFSET               0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define    DEFAULT_WALK_X_OFFSET                0xFFF6 // -10 mm      
+#define    DEFAULT_WALK_Y_OFFSET                0x0005 // 5 mm
+#define    DEFAULT_WALK_Z_OFFSET                0x0014 // 20 mm
+#define    DEFAULT_WALK_ROLL_OFFSET             0x0000 // 0 degrees in fixed point format 5 (1+4) | 3
+#define    DEFAULT_WALK_PITCH_OFFSET            0x0000 // 0 degrees in fixed point format 5 (1+4) | 3
+#define    DEFAULT_WALK_YAW_OFFSET              0x0000 // 0 degrees in fixed point format 5 (1+4) | 3
+#define    DEFAULT_WALK_HIP_PITCH_OFF           0x34B6 // 13.18 degrees in fixed point format 6 (1+5) | 10 
+#define    DEFAULT_WALK_PERIOD_TIME             0x0258 // 600 ms
+#define    DEFAULT_WALK_DSP_RATIO               0x0019 // 0.1 in fixed point format 0 | 8
+#define    DEFAULT_WALK_STEP_FW_BW_RATIO        0x004C // 0.3 in fixed point format 0 | 8
+#define    DEFAULT_WALK_FOOT_HEIGHT             0x0028 // 40 mm
+#define    DEFAULT_WALK_SWING_RIGHT_LEFT        0x0014 // 20 mm
+#define    DEFAULT_WALK_SWING_TOP_DOWN          0x0005 // 5 mm
+#define    DEFAULT_WALK_PELVIS_OFFSET           0x0018 // 3 degrees in fixed point format 5 (1+4) | 3
+#define    DEFAULT_WALK_ARM_SWING_GAIN          0x0030 // 1.5 in fixed point format 3 | 5
+#define    DEFAULT_WALK_MAX_VEL                 0x0016 // 20 mm/s
+#define    DEFAULT_WALK_MAX_ROT_VEL             0x0040 // 8 degrees/s in fixed point format 5 | 3
+#define    DEFAULT_HEAD_PAN_P                   0x028F // 0.01 in fixed point format 0|16 
+#define    DEFAULT_HEAD_PAN_I                   0x0000 // 0.0 in fixed point format 0|16
+#define    DEFAULT_HEAD_PAN_D                   0x0000 // 0.0 in fixed point format 0|16
+#define    DEFAULT_HEAD_PAN_I_CLAMP             0x0000 // 0.0 in fixed point format 9|7
+#define    DEFAULT_HEAD_TILT_P                  0x028F // 0.01 in fixed point format 0|16
+#define    DEFAULT_HEAD_TILT_I                  0x0000 // 0.0 in fixed point format 0|16
+#define    DEFAULT_HEAD_TILT_D                  0x0000 // 0.0 in fixed point format 0|16
+#define    DEFAULT_HEAD_TILT_I_CLAMP            0x0000 // 0.0 in fixed point format 9|7
+#define    DEFAULT_GRIPPER_LEFT_ID              0x0017 // ID 23 for the left gripper
+#define    DEFAULT_GRIPPER_LEFT_MAX_ANGLE       0x0F00 // 30 in fixed point format 9|7
+#define    DEFAULT_GRIPPER_LEFT_MIN_ANGLE       0xF100 // -30 in fixed point format 9|7
+#define    DEFAULT_GRIPPER_LEFT_MAX_FORCE       0x03FF // 1023 max force in binary format
+#define    DEFAULT_GRIPPER_RIGHT_ID             0x0018 // ID 24 for the left gripper
+#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      0x03FF // 1023 max force in binary format
+#define    DEFAULT_SMART_CHARGER_PERIOD         0x05DC //1500 ms 
 
   
 
diff --git a/src/eeprom.c b/src/eeprom.c
index d511af2..4fa6f8e 100755
--- a/src/eeprom.c
+++ b/src/eeprom.c
@@ -53,108 +53,108 @@
 uint16_t DataVar = 0;
 
 /* Virtual address defined by the user: 0xFFFF value is prohibited */
-uint16_t eeprom_data[] __attribute__ ((section (".eeprom")))={VALID_PAGE,                            0xFFFF,
-                                                              DEFAULT_DEVICE_MODEL&0xFF,             DEVICE_MODEL_OFFSET,// model number LSB
-                                                              DEFAULT_DEVICE_MODEL>>8,               DEVICE_MODEL_OFFSET+1,// model number MSB
-                                                              DEFAULT_FIRMWARE_VERSION,              FIRMWARE_VERSION_OFFSET,// firmware version
-                                                              DEFAULT_DEVICE_ID,                     DEVICE_ID_OFFSET,// default device id
-                                                              DEFAULT_BAUDRATE,                      BAUDRATE_OFFSET,// default baudrate 
-                                                              DEFAULT_RETURN_DELAY,                  RETURN_DELAY_OFFSET,// return delay time
-                                                              DEFAULT_MM_PERIOD&0xFF,                MM_PERIOD_OFFSET,
-                                                              DEFAULT_MM_PERIOD>>8,                  MM_PERIOD_OFFSET+1,
-                                                              DEFAULT_BAL_KNEE_GAIN&0xFF,            MM_BAL_KNEE_GAIN_OFFSET,
-                                                              DEFAULT_BAL_KNEE_GAIN>>8,              MM_BAL_KNEE_GAIN_OFFSET+1,
-                                                              DEFAULT_BAL_ANKLE_ROLL_GAIN&0xFF,      MM_BAL_ANKLE_ROLL_GAIN_OFFSET,
-                                                              DEFAULT_BAL_ANKLE_ROLL_GAIN>>8,        MM_BAL_ANKLE_ROLL_GAIN_OFFSET+1,
-                                                              DEFAULT_BAL_ANKLE_PITCH_GAIN&0xFF,     MM_BAL_ANKLE_PITCH_GAIN_OFFSET,
-                                                              DEFAULT_BAL_ANKLE_PITCH_GAIN>>8,       MM_BAL_ANKLE_PITCH_GAIN_OFFSET+1,
-                                                              DEFAULT_BAL_HIP_ROLL_GAIN&0xFF,        MM_BAL_HIP_ROLL_GAIN_OFFSET,
-                                                              DEFAULT_BAL_HIP_ROLL_GAIN>>8,          MM_BAL_HIP_ROLL_GAIN_OFFSET+1,
-                                                              DEFAULT_RETURN_LEVEL,                  RETURN_LEVEL_OFFSET,// return level
-                                                              DEFAULT_SERVO0_OFFSET,                 MM_SERVO0_OFFSET,
-                                                              DEFAULT_SERVO1_OFFSET,                 MM_SERVO1_OFFSET,
-                                                              DEFAULT_SERVO2_OFFSET,                 MM_SERVO2_OFFSET,
-                                                              DEFAULT_SERVO3_OFFSET,                 MM_SERVO3_OFFSET,
-                                                              DEFAULT_SERVO4_OFFSET,                 MM_SERVO4_OFFSET,
-                                                              DEFAULT_SERVO5_OFFSET,                 MM_SERVO5_OFFSET,
-                                                              DEFAULT_SERVO6_OFFSET,                 MM_SERVO6_OFFSET,
-                                                              DEFAULT_SERVO7_OFFSET,                 MM_SERVO7_OFFSET,
-                                                              DEFAULT_SERVO8_OFFSET,                 MM_SERVO8_OFFSET,
-                                                              DEFAULT_SERVO9_OFFSET,                 MM_SERVO9_OFFSET,
-                                                              DEFAULT_SERVO10_OFFSET,                MM_SERVO10_OFFSET,
-                                                              DEFAULT_SERVO11_OFFSET,                MM_SERVO11_OFFSET,
-                                                              DEFAULT_SERVO12_OFFSET,                MM_SERVO12_OFFSET,
-                                                              DEFAULT_SERVO13_OFFSET,                MM_SERVO13_OFFSET,
-                                                              DEFAULT_SERVO14_OFFSET,                MM_SERVO14_OFFSET,
-                                                              DEFAULT_SERVO15_OFFSET,                MM_SERVO15_OFFSET,
-                                                              DEFAULT_SERVO16_OFFSET,                MM_SERVO16_OFFSET,
-                                                              DEFAULT_SERVO17_OFFSET,                MM_SERVO17_OFFSET,
-                                                              DEFAULT_SERVO18_OFFSET,                MM_SERVO18_OFFSET,
-                                                              DEFAULT_SERVO19_OFFSET,                MM_SERVO19_OFFSET,
-                                                              DEFAULT_SERVO20_OFFSET,                MM_SERVO20_OFFSET,
-                                                              DEFAULT_SERVO21_OFFSET,                MM_SERVO21_OFFSET,
-                                                              DEFAULT_SERVO22_OFFSET,                MM_SERVO22_OFFSET,
-                                                              DEFAULT_SERVO23_OFFSET,                MM_SERVO23_OFFSET,
-                                                              DEFAULT_SERVO24_OFFSET,                MM_SERVO24_OFFSET,
-                                                              DEFAULT_SERVO25_OFFSET,                MM_SERVO25_OFFSET,
-                                                              DEFAULT_SERVO26_OFFSET,                MM_SERVO26_OFFSET,
-                                                              DEFAULT_SERVO27_OFFSET,                MM_SERVO27_OFFSET,
-                                                              DEFAULT_SERVO28_OFFSET,                MM_SERVO28_OFFSET,
-                                                              DEFAULT_SERVO29_OFFSET,                MM_SERVO29_OFFSET,
-                                                              DEFAULT_SERVO30_OFFSET,                MM_SERVO30_OFFSET,
-                                                              DEFAULT_SERVO31_OFFSET,                MM_SERVO31_OFFSET,
-                                                              DEFAULT_WALK_X_OFFSET,                 WALK_X_OFFSET,
-                                                              DEFAULT_WALK_Y_OFFSET,                 WALK_Y_OFFSET,
-                                                              DEFAULT_WALK_Z_OFFSET,                 WALK_Z_OFFSET,
-                                                              DEFAULT_WALK_ROLL_OFFSET,              WALK_ROLL_OFFSET,
-                                                              DEFAULT_WALK_PITCH_OFFSET,             WALK_PITCH_OFFSET,
-                                                              DEFAULT_WALK_YAW_OFFSET,               WALK_YAW_OFFSET,
-                                                              DEFAULT_WALK_HIP_PITCH_OFF&0xFF,       WALK_HIP_PITCH_OFF,
-                                                              DEFAULT_WALK_HIP_PITCH_OFF>>8,         WALK_HIP_PITCH_OFF+1,
-                                                              DEFAULT_WALK_PERIOD_TIME&0xFF,         WALK_PERIOD_TIME,
-                                                              DEFAULT_WALK_PERIOD_TIME>>8,           WALK_PERIOD_TIME+1,
-                                                              DEFAULT_WALK_DSP_RATIO,                WALK_DSP_RATIO,
-                                                              DEFAULT_WALK_STEP_FW_BW_RATIO,         WALK_STEP_FW_BW_RATIO,
-                                                              DEFAULT_WALK_FOOT_HEIGHT,              WALK_FOOT_HEIGHT,
-                                                              DEFAULT_WALK_SWING_RIGHT_LEFT,         WALK_SWING_RIGHT_LEFT,
-                                                              DEFAULT_WALK_SWING_TOP_DOWN,           WALK_SWING_TOP_DOWN,
-                                                              DEFAULT_WALK_PELVIS_OFFSET,            WALK_PELVIS_OFFSET,
-                                                              DEFAULT_WALK_ARM_SWING_GAIN,           WALK_ARM_SWING_GAIN,
-                                                              DEFAULT_WALK_MAX_VEL,                  WALK_MAX_VEL,
-                                                              DEFAULT_WALK_MAX_ROT_VEL,              WALK_MAX_ROT_VEL,
-                                                              DEFAULT_HEAD_PAN_P&0xFF,               HEAD_PAN_P,
-                                                              DEFAULT_HEAD_PAN_P>>8,                 HEAD_PAN_P+1,
-                                                              DEFAULT_HEAD_PAN_I&0xFF,               HEAD_PAN_I,
-                                                              DEFAULT_HEAD_PAN_I>>8,                 HEAD_PAN_I+1,
-                                                              DEFAULT_HEAD_PAN_D&0xFF,               HEAD_PAN_D,
-                                                              DEFAULT_HEAD_PAN_D>>8,                 HEAD_PAN_D+1,
-                                                              DEFAULT_HEAD_PAN_I_CLAMP&0xFF,         HEAD_PAN_I_CLAMP,
-                                                              DEFAULT_HEAD_PAN_I_CLAMP>>8,           HEAD_PAN_I_CLAMP+1,
-                                                              DEFAULT_HEAD_PAN_D>>8,                 HEAD_PAN_D+1,
-                                                              DEFAULT_HEAD_TILT_P&0xFF,              HEAD_TILT_P,
-                                                              DEFAULT_HEAD_TILT_P>>8,                HEAD_TILT_P+1,
-                                                              DEFAULT_HEAD_TILT_I&0xFF,              HEAD_TILT_I,
-                                                              DEFAULT_HEAD_TILT_I>>8,                HEAD_TILT_I+1,
-                                                              DEFAULT_HEAD_TILT_D&0xFF,              HEAD_TILT_D,
-                                                              DEFAULT_HEAD_TILT_D>>8,                HEAD_TILT_D+1,
-                                                              DEFAULT_HEAD_TILT_I_CLAMP&0xFF,        HEAD_TILT_I_CLAMP,
-                                                              DEFAULT_HEAD_TILT_I_CLAMP>>8,          HEAD_TILT_I_CLAMP+1,
-                                                              DEFAULT_GRIPPER_LEFT_ID,               GRIPPER_LEFT_ID,
-                                                              DEFAULT_GRIPPER_LEFT_MAX_ANGLE&0xFF,   GRIPPER_LEFT_MAX_ANGLE,
-                                                              DEFAULT_GRIPPER_LEFT_MAX_ANGLE>>8,     GRIPPER_LEFT_MAX_ANGLE+1,
-                                                              DEFAULT_GRIPPER_LEFT_MIN_ANGLE&0xFF,   GRIPPER_LEFT_MIN_ANGLE,
-                                                              DEFAULT_GRIPPER_LEFT_MIN_ANGLE>>8,     GRIPPER_LEFT_MIN_ANGLE+1,
-                                                              DEFAULT_GRIPPER_LEFT_MAX_FORCE&0xFF,   GRIPPER_LEFT_MAX_FORCE,
-                                                              DEFAULT_GRIPPER_LEFT_MAX_FORCE>>8,     GRIPPER_LEFT_MAX_FORCE+1,
-                                                              DEFAULT_GRIPPER_RIGHT_ID,              GRIPPER_RIGHT_ID,
-                                                              DEFAULT_GRIPPER_RIGHT_MAX_ANGLE&0xFF,  GRIPPER_RIGHT_MAX_ANGLE,
-                                                              DEFAULT_GRIPPER_RIGHT_MAX_ANGLE>>8,    GRIPPER_RIGHT_MAX_ANGLE+1,
-                                                              DEFAULT_GRIPPER_RIGHT_MIN_ANGLE&0xFF,  GRIPPER_RIGHT_MIN_ANGLE,
-                                                              DEFAULT_GRIPPER_RIGHT_MIN_ANGLE>>8,    GRIPPER_RIGHT_MIN_ANGLE+1,
-                                                              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};
+uint16_t eeprom_data[] __attribute__ ((section (".eeprom")))={VALID_PAGE,                                0xFFFF,
+                                                              DEFAULT_DEVICE_MODEL&0xFF,                 DEVICE_MODEL_OFFSET,// model number LSB
+                                                              DEFAULT_DEVICE_MODEL>>8,                   DEVICE_MODEL_OFFSET+1,// model number MSB
+                                                              DEFAULT_FIRMWARE_VERSION,                  FIRMWARE_VERSION_OFFSET,// firmware version
+                                                              DEFAULT_DEVICE_ID,                         DEVICE_ID_OFFSET,// default device id
+                                                              DEFAULT_BAUDRATE,                          BAUDRATE_OFFSET,// default baudrate 
+                                                              DEFAULT_RETURN_DELAY,                      RETURN_DELAY_OFFSET,// return delay time
+                                                              DEFAULT_MM_PERIOD&0xFF,                    MM_PERIOD_OFFSET,
+                                                              DEFAULT_MM_PERIOD>>8,                      MM_PERIOD_OFFSET+1,
+                                                              DEFAULT_BAL_KNEE_GAIN&0xFF,                MM_BAL_KNEE_GAIN_OFFSET,
+                                                              DEFAULT_BAL_KNEE_GAIN>>8,                  MM_BAL_KNEE_GAIN_OFFSET+1,
+                                                              DEFAULT_BAL_ANKLE_ROLL_GAIN&0xFF,          MM_BAL_ANKLE_ROLL_GAIN_OFFSET,
+                                                              DEFAULT_BAL_ANKLE_ROLL_GAIN>>8,            MM_BAL_ANKLE_ROLL_GAIN_OFFSET+1,
+                                                              DEFAULT_BAL_ANKLE_PITCH_GAIN&0xFF,         MM_BAL_ANKLE_PITCH_GAIN_OFFSET,
+                                                              DEFAULT_BAL_ANKLE_PITCH_GAIN>>8,           MM_BAL_ANKLE_PITCH_GAIN_OFFSET+1,
+                                                              DEFAULT_BAL_HIP_ROLL_GAIN&0xFF,            MM_BAL_HIP_ROLL_GAIN_OFFSET,
+                                                              DEFAULT_BAL_HIP_ROLL_GAIN>>8,              MM_BAL_HIP_ROLL_GAIN_OFFSET+1,
+                                                              DEFAULT_RETURN_LEVEL,                      RETURN_LEVEL_OFFSET,// return level
+                                                              DEFAULT_SERVO0_OFFSET,                     MM_SERVO0_OFFSET,
+                                                              DEFAULT_SERVO1_OFFSET,                     MM_SERVO1_OFFSET,
+                                                              DEFAULT_SERVO2_OFFSET,                     MM_SERVO2_OFFSET,
+                                                              DEFAULT_SERVO3_OFFSET,                     MM_SERVO3_OFFSET,
+                                                              DEFAULT_SERVO4_OFFSET,                     MM_SERVO4_OFFSET,
+                                                              DEFAULT_SERVO5_OFFSET,                     MM_SERVO5_OFFSET,
+                                                              DEFAULT_SERVO6_OFFSET,                     MM_SERVO6_OFFSET,
+                                                              DEFAULT_SERVO7_OFFSET,                     MM_SERVO7_OFFSET,
+                                                              DEFAULT_SERVO8_OFFSET,                     MM_SERVO8_OFFSET,
+                                                              DEFAULT_SERVO9_OFFSET,                     MM_SERVO9_OFFSET,
+                                                              DEFAULT_SERVO10_OFFSET,                    MM_SERVO10_OFFSET,
+                                                              DEFAULT_SERVO11_OFFSET,                    MM_SERVO11_OFFSET,
+                                                              DEFAULT_SERVO12_OFFSET,                    MM_SERVO12_OFFSET,
+                                                              DEFAULT_SERVO13_OFFSET,                    MM_SERVO13_OFFSET,
+                                                              DEFAULT_SERVO14_OFFSET,                    MM_SERVO14_OFFSET,
+                                                              DEFAULT_SERVO15_OFFSET,                    MM_SERVO15_OFFSET,
+                                                              DEFAULT_SERVO16_OFFSET,                    MM_SERVO16_OFFSET,
+                                                              DEFAULT_SERVO17_OFFSET,                    MM_SERVO17_OFFSET,
+                                                              DEFAULT_SERVO18_OFFSET,                    MM_SERVO18_OFFSET,
+                                                              DEFAULT_SERVO19_OFFSET,                    MM_SERVO19_OFFSET,
+                                                              DEFAULT_SERVO20_OFFSET,                    MM_SERVO20_OFFSET,
+                                                              DEFAULT_SERVO21_OFFSET,                    MM_SERVO21_OFFSET,
+                                                              DEFAULT_SERVO22_OFFSET,                    MM_SERVO22_OFFSET,
+                                                              DEFAULT_SERVO23_OFFSET,                    MM_SERVO23_OFFSET,
+                                                              DEFAULT_SERVO24_OFFSET,                    MM_SERVO24_OFFSET,
+                                                              DEFAULT_SERVO25_OFFSET,                    MM_SERVO25_OFFSET,
+                                                              DEFAULT_SERVO26_OFFSET,                    MM_SERVO26_OFFSET,
+                                                              DEFAULT_SERVO27_OFFSET,                    MM_SERVO27_OFFSET,
+                                                              DEFAULT_SERVO28_OFFSET,                    MM_SERVO28_OFFSET,
+                                                              DEFAULT_SERVO29_OFFSET,                    MM_SERVO29_OFFSET,
+                                                              DEFAULT_SERVO30_OFFSET,                    MM_SERVO30_OFFSET,
+                                                              DEFAULT_SERVO31_OFFSET,                    MM_SERVO31_OFFSET,
+                                                              DEFAULT_WALK_X_OFFSET,                     WALK_X_OFFSET,
+                                                              DEFAULT_WALK_Y_OFFSET,                     WALK_Y_OFFSET,
+                                                              DEFAULT_WALK_Z_OFFSET,                     WALK_Z_OFFSET,
+                                                              DEFAULT_WALK_ROLL_OFFSET,                  WALK_ROLL_OFFSET,
+                                                              DEFAULT_WALK_PITCH_OFFSET,                 WALK_PITCH_OFFSET,
+                                                              DEFAULT_WALK_YAW_OFFSET,                   WALK_YAW_OFFSET,
+                                                              DEFAULT_WALK_HIP_PITCH_OFF&0xFF,           WALK_HIP_PITCH_OFF,
+                                                              DEFAULT_WALK_HIP_PITCH_OFF>>8,             WALK_HIP_PITCH_OFF+1,
+                                                              DEFAULT_WALK_PERIOD_TIME&0xFF,             WALK_PERIOD_TIME,
+                                                              DEFAULT_WALK_PERIOD_TIME>>8,               WALK_PERIOD_TIME+1,
+                                                              DEFAULT_WALK_DSP_RATIO,                    WALK_DSP_RATIO,
+                                                              DEFAULT_WALK_STEP_FW_BW_RATIO,             WALK_STEP_FW_BW_RATIO,
+                                                              DEFAULT_WALK_FOOT_HEIGHT,                  WALK_FOOT_HEIGHT,
+                                                              DEFAULT_WALK_SWING_RIGHT_LEFT,             WALK_SWING_RIGHT_LEFT,
+                                                              DEFAULT_WALK_SWING_TOP_DOWN,               WALK_SWING_TOP_DOWN,
+                                                              DEFAULT_WALK_PELVIS_OFFSET,                WALK_PELVIS_OFFSET,
+                                                              DEFAULT_WALK_ARM_SWING_GAIN,               WALK_ARM_SWING_GAIN,
+                                                              DEFAULT_WALK_MAX_VEL,                      WALK_MAX_VEL,
+                                                              DEFAULT_WALK_MAX_ROT_VEL,                  WALK_MAX_ROT_VEL,
+                                                              DEFAULT_HEAD_PAN_P&0xFF,                   HEAD_PAN_P,
+                                                              DEFAULT_HEAD_PAN_P>>8,                     HEAD_PAN_P+1,
+                                                              DEFAULT_HEAD_PAN_I&0xFF,                   HEAD_PAN_I,
+                                                              DEFAULT_HEAD_PAN_I>>8,                     HEAD_PAN_I+1,
+                                                              DEFAULT_HEAD_PAN_D&0xFF,                   HEAD_PAN_D,
+                                                              DEFAULT_HEAD_PAN_D>>8,                     HEAD_PAN_D+1,
+                                                              DEFAULT_HEAD_PAN_I_CLAMP&0xFF,             HEAD_PAN_I_CLAMP,
+                                                              DEFAULT_HEAD_PAN_I_CLAMP>>8,               HEAD_PAN_I_CLAMP+1,
+                                                              DEFAULT_HEAD_PAN_D>>8,                     HEAD_PAN_D+1,
+                                                              DEFAULT_HEAD_TILT_P&0xFF,                  HEAD_TILT_P,
+                                                              DEFAULT_HEAD_TILT_P>>8,                    HEAD_TILT_P+1,
+                                                              DEFAULT_HEAD_TILT_I&0xFF,                  HEAD_TILT_I,
+                                                              DEFAULT_HEAD_TILT_I>>8,                    HEAD_TILT_I+1,
+                                                              DEFAULT_HEAD_TILT_D&0xFF,                  HEAD_TILT_D,
+                                                              DEFAULT_HEAD_TILT_D>>8,                    HEAD_TILT_D+1,
+                                                              DEFAULT_HEAD_TILT_I_CLAMP&0xFF,            HEAD_TILT_I_CLAMP,
+                                                              DEFAULT_HEAD_TILT_I_CLAMP>>8,              HEAD_TILT_I_CLAMP+1,
+                                                              DEFAULT_GRIPPER_LEFT_ID,                   GRIPPER_LEFT_ID,
+                                                              DEFAULT_GRIPPER_LEFT_MAX_ANGLE&0xFF,       GRIPPER_LEFT_MAX_ANGLE,
+                                                              DEFAULT_GRIPPER_LEFT_MAX_ANGLE>>8,         GRIPPER_LEFT_MAX_ANGLE+1,
+                                                              DEFAULT_GRIPPER_LEFT_MIN_ANGLE&0xFF,       GRIPPER_LEFT_MIN_ANGLE,
+                                                              DEFAULT_GRIPPER_LEFT_MIN_ANGLE>>8,         GRIPPER_LEFT_MIN_ANGLE+1,
+                                                              DEFAULT_GRIPPER_LEFT_MAX_FORCE&0xFF,       GRIPPER_LEFT_MAX_FORCE,
+                                                              DEFAULT_GRIPPER_LEFT_MAX_FORCE>>8,         GRIPPER_LEFT_MAX_FORCE+1,
+                                                              DEFAULT_GRIPPER_RIGHT_ID,                  GRIPPER_RIGHT_ID,
+                                                              DEFAULT_GRIPPER_RIGHT_MAX_ANGLE&0xFF,      GRIPPER_RIGHT_MAX_ANGLE,
+                                                              DEFAULT_GRIPPER_RIGHT_MAX_ANGLE>>8,        GRIPPER_RIGHT_MAX_ANGLE+1,
+                                                              DEFAULT_GRIPPER_RIGHT_MIN_ANGLE&0xFF,      GRIPPER_RIGHT_MIN_ANGLE,
+                                                              DEFAULT_GRIPPER_RIGHT_MIN_ANGLE>>8,        GRIPPER_RIGHT_MIN_ANGLE+1,
+                                                              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};
 
 /* Private function prototypes -----------------------------------------------*/
 /* Private functions ---------------------------------------------------------*/
diff --git a/src/motion_manager.c b/src/motion_manager.c
index eeb4d17..6e7768a 100755
--- a/src/motion_manager.c
+++ b/src/motion_manager.c
@@ -11,6 +11,7 @@
 #include "imu.h"
 #include "smart_charger.h"
 #include "dyn_battery.h"
+#include "gpio.h"
 
 #define MANAGER_TIMER                   TIM5
 #define ENABLE_MANAGER_TIMER_CLK        __HAL_RCC_TIM5_CLK_ENABLE()
@@ -332,7 +333,7 @@ void manager_init(uint16_t period_us)
                               smart_charger_set_master(&darwin_dyn_master);          //set bus Dynamixel master version 
                               // Set smart charger's memory map
                               dyn_master_read_table(&darwin_dyn_master,ram_data[DARWIN_SMART_CHARGER_ID],BATTERY_AVG_TIME_EMPTY_L,6,&ram_data[DARWIN_SMART_CHARGER_AVG_TIME_EMPTY_L]);
-                              dyn_master_read_byte(&darwin_dyn_master,ram_data[DARWIN_SMART_CHARGER_ID],BATTERY_INPUT_MAX_CURRENT_L,&ram_data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_L]);
+                              dyn_master_write_table(&darwin_dyn_master,ram_data[DARWIN_SMART_CHARGER_ID],BATTERY_INPUT_MAX_CURRENT_L,2,&ram_data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_L]);
                               break;
       default: break;
     }
@@ -389,12 +390,13 @@ void manager_init(uint16_t period_us)
     dyn_master_read_word(&darwin_dyn_master_v2,servo_ids[i],BATTERY_MODEL_NUMBER_L,&model);
     switch(model)
     {
+     // gpio_set_led(LED_RX);
       case DYN_BATTERY_MODEL: ram_data[DARWIN_SMART_CHARGER_CNTRL]=SMART_CHARGER_DET; //smart charger detected
                               ram_data[DARWIN_SMART_CHARGER_ID] = servo_ids[i];       //smart charger ID
                               smart_charger_set_master(&darwin_dyn_master_v2);       //Set bus Dynamixel master version 
                               // Set smart charger's memory map
                               dyn_master_read_table(&darwin_dyn_master_v2,ram_data[DARWIN_SMART_CHARGER_ID],BATTERY_AVG_TIME_EMPTY_L,6,&ram_data[DARWIN_SMART_CHARGER_AVG_TIME_EMPTY_L]);
-                              dyn_master_read_byte(&darwin_dyn_master_v2,ram_data[DARWIN_SMART_CHARGER_ID],BATTERY_INPUT_MAX_CURRENT_L,&ram_data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_L]);
+                              //dyn_master_write_table(&darwin_dyn_master_v2,ram_data[DARWIN_SMART_CHARGER_ID],BATTERY_INPUT_MAX_CURRENT_L,2,&ram_data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_L]);
                               break;
       default: break;
     }
diff --git a/src/ram.c b/src/ram.c
index f2c505d..e061c05 100755
--- a/src/ram.c
+++ b/src/ram.c
@@ -151,6 +151,7 @@ void ram_init(void)
     ram_data[SMART_CHARGER_PERIOD]=(uint8_t)(eeprom_data&0x00FF);
   if(EE_ReadVariable(SMART_CHARGER_PERIOD+1,&eeprom_data)==0)
     ram_data[SMART_CHARGER_PERIOD+1]=(uint8_t)(eeprom_data&0x00FF);
+
 }
 
 inline void ram_read_byte(uint16_t address,uint8_t *data)
diff --git a/src/smart_charger.c b/src/smart_charger.c
index 3812797..03a9ed8 100644
--- a/src/smart_charger.c
+++ b/src/smart_charger.c
@@ -9,18 +9,10 @@ uint8_t smart_charger_id;       //smart charger dynamixel ID
 uint8_t smart_charger_enabled;  //smart charger module enable signal
 uint8_t smart_charger_detected; //smart charger detect signal
 uint16_t smart_charger_period;  //smart charger period value in ms
-//uint16_t counter;               //for read access operations
 uint16_t smart_charger_count;   //counter = sc period / mm period --> Default: (1500ms*1000)/7800us
 uint8_t smart_charger_write;    //write smart charger signal
 uint8_t init_regs[6]={0xFF,0xFF,0xFF,0xFF,0xFF,0xFF};
 
-#define FIFOSIZE 32
-unsigned char *infifo;              //Pointer to fifo position where to store next input data
-unsigned char *outfifo;             //Pointer to fifo position where is stored the data to be written
-unsigned char numdata;              //Number of elements stored in the fifo
-unsigned char write_fifo[FIFOSIZE]; //Vector with data to write (data_write)
-unsigned char current[2];
-
 //---------------------------------------
 
 TDynamixelMaster *dyn_battery_master;
@@ -31,12 +23,9 @@ void smart_charger_init(uint16_t period_us)
   smart_charger_id = ram_data[DARWIN_SMART_CHARGER_ID];
   smart_charger_enabled = ram_data[DARWIN_SMART_CHARGER_CNTRL]&SMART_CHARGER_EN;
   smart_charger_detected = ram_data[DARWIN_SMART_CHARGER_CNTRL]&SMART_CHARGER_DET;
- // counter = 0;
-  smart_charger_period = (ram_data[DARWIN_SMART_CHARGER_PERIOD_L]) + ((ram_data[DARWIN_SMART_CHARGER_PERIOD_H])<<8);
+  smart_charger_period = ram_data[DARWIN_SMART_CHARGER_PERIOD_L] + (ram_data[DARWIN_SMART_CHARGER_PERIOD_H]<<8);
   smart_charger_count = (smart_charger_period*1000)/period_us;                                                                                
   smart_charger_write = 0x00;
-  infifo=outfifo=&write_fifo[0];
-  numdata=0; //fifo empty
   ram_write_table(DARWIN_SMART_CHARGER_AVG_TIME_EMPTY_L, 6, init_regs);
 }
 
@@ -93,7 +82,7 @@ void smart_charger_process_read_cmd(unsigned short int address,unsigned short in
 
 void smart_charger_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
 {
-  uint16_t period;//,i;
+  uint16_t period;
   
 
   //Enable/Disable smart charger
@@ -114,28 +103,8 @@ void smart_charger_process_write_cmd(unsigned short int address,unsigned short i
   //Write Battery limit current
   if(ram_in_range(DARWIN_SMART_CHARGER_LIMIT_CURRENT_L,address,length) && ram_in_range(DARWIN_SMART_CHARGER_LIMIT_CURRENT_H,address,length))
   {
-    current[0]=data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_L-address];
-    current[1]=data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_H-address];
-    
-    
- /*   if(infifo==&write_fifo[FIFOSIZE]);
-      infifo=&write_fifo[0]; //go to first position of fifo
-    if(numdata<FIFOSIZE)  //free space in fifo
-    { 
-      for(i=DARWIN_SMART_CHARGER_LIMIT_CURRENT_L;i<=DARWIN_SMART_CHARGER_LIMIT_CURRENT_H;i++)
-      {
-        *infifo=data[i-address];
-        infifo++;
-        numdata++;
-      }
- */     
-    /* *infifo=data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_L-address]; //copy first byte to fifo
-     numdata++;
-     infifo++;
-     *infifo=data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_H-address]; //copy second byte to fifo
-     numdata++;
-     infifo++; */
- //   }
+    ram_data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_L]=data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_L-address];
+    ram_data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_H]=data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_H-address];
     smart_charger_write = 0x01;
   }
 }
@@ -144,38 +113,27 @@ void smart_charger_process_write_cmd(unsigned short int address,unsigned short i
 // motion manager interface function
 void smart_charger_process(void)
 { 
+  //gpio_set_led(LED_3);
   uint8_t error;
-  static uint16_t counter;
+  static uint16_t counter=0;
   
   counter++;
 
   //Write smart_charger - Battery limit current (EEPROM)
   if(smart_charger_detected && smart_charger_enabled && smart_charger_write && counter!=smart_charger_count)
   {
-    error=dyn_master_write_table(dyn_battery_master,smart_charger_id,BATTERY_INPUT_MAX_CURRENT_L,2, current);
-      if(error==DYN_SUCCESS){
-/*    dyn_master_write_table(dyn_battery_master,smart_charger_id,BATTERY_INPUT_MAX_CURRENT_L,2,outfifo);
-    if(outfifo==&write_fifo[FIFOSIZE-1])
-      outfifo=&write_fifo[0]; //go to first position of fifo
-    else 
-      outfifo=outfifo+2; 
-    numdata=numdata-2;
-    if(numdata==0) //empty fifo
- */
+    error=dyn_master_write_table(dyn_battery_master,smart_charger_id,BATTERY_INPUT_MAX_CURRENT_L,2,&ram_data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_L]);
+      if(error==DYN_SUCCESS)
         smart_charger_write = 0x00;
-      }
   }
   
   //Read smart charger - time to empty, time to full and battery_status  
   if(smart_charger_detected && smart_charger_enabled && counter==smart_charger_count)
   {
-    
     error=dyn_master_read_table(dyn_battery_master,smart_charger_id,BATTERY_AVG_TIME_EMPTY_L,6,&ram_data[DARWIN_SMART_CHARGER_AVG_TIME_EMPTY_L]);
     if(error!=DYN_SUCCESS){
+     // gpio_set_led(LED_TX);
       ram_write_table(DARWIN_SMART_CHARGER_AVG_TIME_EMPTY_L, 6, init_regs);
-      gpio_set_led(LED_RX); //naranja
-    }else{
-      gpio_set_led(LED_3);//verde
     }
     counter = 0; 
   }
-- 
GitLab


From f2be8d2926c9a7d84b89e96cce65f7a8f527e3a9 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Wed, 31 May 2017 14:36:05 +0200
Subject: [PATCH 03/21] Added support to detect when the robot falls using the
 accelerometer.

---
 include/darwin_registers.h |  6 ++++--
 include/imu.h              |  1 +
 include/motion_manager.h   |  6 ++++++
 src/imu.c                  | 37 +++++++++++++++++++++--------------
 src/motion_manager.c       | 40 ++++++++++++++++++++++++++++++++++++++
 5 files changed, 73 insertions(+), 17 deletions(-)

diff --git a/include/darwin_registers.h b/include/darwin_registers.h
index 4ed2722..c91a832 100644
--- a/include/darwin_registers.h
+++ b/include/darwin_registers.h
@@ -272,8 +272,8 @@ typedef enum {
   DARWIN_IMU_ACCEL_Z_H             = 0x0149,
 
   DARWIN_MM_NUM_SERVOS             = 0x014A,
-  DARWIN_MM_CNTRL                  = 0x014B, //   bit 7  | bit 6 | bit 5 | bit 4 | bit 3 |     bit 2    |      bit 1      |    bit 0
-                                             // scanning |       |       |       |       | Enable power | Enable balance  | Enable manager
+  DARWIN_MM_CNTRL                  = 0x014B, //   bit 7  |   bit 6  |   bit 5  | bit 4 | bit 3 |     bit 2    |      bit 1      |    bit 0
+                                             // scanning | fwd fall | bwd fall |       |       | Enable power | Enable balance  | Enable manager
   DARWIN_MM_MODULE_EN0             = 0x014C, //      bit 7      | bit 6 | bit 5 | bit 4 |      bit 3      | bit 2 | bit 1 | bit 0i
                                              // Enable servo 0  |   assigned module     | Enable servo 1  |   assigned module
                                              //                 |     000 -> none       |
@@ -627,6 +627,8 @@ typedef enum {
 #define      MANAGER_EN_BAL          0x02
 #define      MANAGER_EN_PWR          0x04
 #define      MANAGER_SCANNING        0x80
+#define      MANAGER_FWD_FALL        0x40
+#define      MANAGER_BWD_FALL        0x20
 #define      MANAGER_EVEN_SER_EN     0x80
 #define      MANAGER_EVEN_SER_MOD    0x70
 #define      MANAGER_ODD_SER_EN      0x08
diff --git a/include/imu.h b/include/imu.h
index 079304b..f3e1c3d 100755
--- a/include/imu.h
+++ b/include/imu.h
@@ -14,6 +14,7 @@ void imu_stop(void);
 void imu_set_calibration_samples(uint16_t num_samples);
 void imu_start_gyro_cal(void);
 void imu_get_gyro_data(int32_t *x, int32_t *y, int32_t *z);
+void imu_get_accel_data(int32_t *x, int32_t *y, int32_t *z);
 uint8_t imu_is_gyro_calibrated(void);
 // operation functions
 uint8_t imu_in_range(unsigned short int address,unsigned short int length);
diff --git a/include/motion_manager.h b/include/motion_manager.h
index 10f053f..641c39d 100755
--- a/include/motion_manager.h
+++ b/include/motion_manager.h
@@ -38,6 +38,10 @@ typedef enum {MM_NONE          = 0,
               MM_JOINTS        = 3,
               MM_HEAD          = 4} TModules;
 
+typedef enum {MM_FWD_FALL      =0,
+              MM_BWD_FALL      =1, 
+              MM_STANDING      =2} TFall;
+
 typedef struct
 {
   void (*process_fnct)(void);
@@ -86,6 +90,8 @@ inline void manager_disable(void);
 inline uint8_t manager_is_enabled(void);
 void manager_enable_balance(void);
 void manager_disable_balance(void);
+inline uint8_t manager_has_fallen(void);
+TFall manager_get_fallen_position(void);
 inline uint8_t manager_get_num_servos(void);
 void manager_set_module(uint8_t servo_id,TModules module);
 inline TModules manager_get_module(uint8_t servo_id);
diff --git a/src/imu.c b/src/imu.c
index 3d3f047..e792585 100755
--- a/src/imu.c
+++ b/src/imu.c
@@ -31,11 +31,11 @@
 // accelerometer registers
 #define        ACCEL_ID               0x32
 #define        ACCEL_WHO_AM_I         0x0F
-#define        ACCEL_CNTRL_REG1        0x20
-#define        ACCEL_CNTRL_REG2        0x21
-#define        ACCEL_CNTRL_REG3        0x22
-#define        ACCEL_CNTRL_REG4        0x23
-#define        ACCEL_CNTRL_REG5        0x24
+#define        ACCEL_CNTRL_REG1       0x20
+#define        ACCEL_CNTRL_REG2       0x21
+#define        ACCEL_CNTRL_REG3       0x22
+#define        ACCEL_CNTRL_REG4       0x23
+#define        ACCEL_CNTRL_REG5       0x24
 #define        ACCEL_HP_FILTER_RESET  0x25
 #define        ACCEL_REFERENCE        0x26
 #define        ACCEL_STATUS_REG       0x27
@@ -58,11 +58,11 @@
 // gyroscope registers
 #define        GYRO_ID                0xD3
 #define        GYRO_WHO_AM_I          0x0F
-#define        GYRO_CNTRL_REG1         0x20
-#define        GYRO_CNTRL_REG2         0x21
-#define        GYRO_CNTRL_REG3         0x22
-#define        GYRO_CNTRL_REG4         0x23
-#define        GYRO_CNTRL_REG5         0x24
+#define        GYRO_CNTRL_REG1        0x20
+#define        GYRO_CNTRL_REG2        0x21
+#define        GYRO_CNTRL_REG3        0x22
+#define        GYRO_CNTRL_REG4        0x23
+#define        GYRO_CNTRL_REG5        0x24
 #define        GYRO_REFERENCE         0x25
 #define        GYRO_OUT_TEMP          0x26
 #define        GYRO_STATUS_REG        0x27
@@ -72,7 +72,7 @@
 #define        GYRO_OUT_Y_H           0x2B
 #define        GYRO_OUT_Z_L           0x2C
 #define        GYRO_OUT_Z_H           0x2D
-#define        GYRO_FIFO_CNTRL_REG     0x2E
+#define        GYRO_FIFO_CNTRL_REG    0x2E
 #define        GYRO_FIFO_SRC_REG      0x2F
 #define        GYRO_INT1_CFG          0x30
 #define        GYRO_INT1_SRC          0x31
@@ -110,6 +110,7 @@ uint16_t gyro_calibration_num_samples;
 const uint8_t accel_conf_reg=ACCEL_CNTRL_REG1;
 const uint8_t accel_conf_len=5;
 uint8_t accel_conf_data[5];
+int32_t accel_data[3];
 const uint8_t accel_data_reg=ACCEL_OUT_X_L;
 const uint8_t accel_data_len=6;
 
@@ -254,13 +255,12 @@ void imu_accel_get_data(void)
 void imu_accel_convert_data(uint8_t *data_in,uint8_t *data_out)
 {
   uint8_t i;
-  int16_t value;
 
   for(i=0;i<3;i++)
   {
-    value=(data_in[i*2]+(data_in[i*2+1]<<8));
-    data_out[i*2]=value%256;
-    data_out[i*2+1]=value/256;
+    accel_data[i]=(data_in[i*2]+(data_in[i*2+1]<<8));
+    data_out[i*2]=accel_data[i]%256;
+    data_out[i*2+1]=accel_data[i]/256;
   }
 }
 
@@ -603,6 +603,13 @@ void imu_get_gyro_data(int32_t *x, int32_t *y, int32_t *z)
   *z=gyro_data[2];
 }
 
+void imu_get_accel_data(int32_t *x, int32_t *y, int32_t *z)
+{
+  *x=accel_data[0];
+  *y=accel_data[1];
+  *z=accel_data[2];
+}
+
 // operation functions
 uint8_t imu_in_range(unsigned short int address,unsigned short int length)
 {
diff --git a/src/motion_manager.c b/src/motion_manager.c
index 6e7768a..b6bb218 100755
--- a/src/motion_manager.c
+++ b/src/motion_manager.c
@@ -12,6 +12,7 @@
 #include "smart_charger.h"
 #include "dyn_battery.h"
 #include "gpio.h"
+#include <stdlib.h>
 
 #define MANAGER_TIMER                   TIM5
 #define ENABLE_MANAGER_TIMER_CLK        __HAL_RCC_TIM5_CLK_ENABLE()
@@ -132,6 +133,7 @@ void manager_balance(void)
 {
   uint32_t knee_gain,ankle_roll_gain,ankle_pitch_gain,hip_roll_gain;
   int32_t gyro_x,gyro_y,gyro_z;
+  int32_t accel_x,accel_y,accel_z;
 
   if(manager_balance_enabled==0x01)// balance is enabled
   {
@@ -152,6 +154,26 @@ void manager_balance(void)
     manager_balance_offset[R_ANKLE_ROLL]=((((int64_t)gyro_x*(int64_t)ankle_roll_gain)/12000)>>9);
     manager_balance_offset[L_ANKLE_ROLL]=((((int64_t)gyro_x*(int64_t)ankle_roll_gain)/12000)>>9);
   }
+  // fall detection (using accelerometer)
+  imu_get_accel_data(&accel_x,&accel_y,&accel_z);
+  if(abs(accel_y)>abs(accel_z))
+  {
+    if(accel_y>0)
+    {
+      ram_data[DARWIN_MM_CNTRL]&=~(MANAGER_FWD_FALL);
+      ram_data[DARWIN_MM_CNTRL]|=MANAGER_BWD_FALL;
+    }
+    else
+    {
+      ram_data[DARWIN_MM_CNTRL]&=~(MANAGER_BWD_FALL);
+      ram_data[DARWIN_MM_CNTRL]|=MANAGER_FWD_FALL;
+    }
+  }
+  else
+  {
+    ram_data[DARWIN_MM_CNTRL]&=~(MANAGER_FWD_FALL);
+    ram_data[DARWIN_MM_CNTRL]&=~(MANAGER_BWD_FALL);
+  }
 }
 
 // interrupt handlers
@@ -509,6 +531,24 @@ void manager_disable_balance(void)
   ram_data[DARWIN_MM_CNTRL]&=(~MANAGER_EN_BAL);
 }
 
+uint8_t manager_has_fallen(void)
+{
+  if(ram_data[DARWIN_MM_CNTRL]&(MANAGER_FWD_FALL|MANAGER_BWD_FALL))
+    return 0x01;
+  else
+    return 0x00;
+}
+
+TFall manager_get_fallen_position(void)
+{
+  if(ram_data[DARWIN_MM_CNTRL]&MANAGER_FWD_FALL)
+    return MM_FWD_FALL;
+  else if(ram_data[DARWIN_MM_CNTRL]&MANAGER_BWD_FALL)
+    return MM_BWD_FALL;
+  else
+    return MM_STANDING;
+}
+
 inline uint8_t manager_get_num_servos(void)
 {
   return manager_num_servos;
-- 
GitLab


From 0210fa1dd083694a998dd31906d5aed3c8b039f0 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Wed, 28 Jun 2017 17:40:03 +0200
Subject: [PATCH 04/21] Added support for the gripper used for the smart
 charger application.

---
 include/darwin_registers.h |  36 +++++---
 include/eeprom_init.h      |   6 +-
 include/grippers.h         |   4 +-
 include/motion_manager.h   |  12 ++-
 src/eeprom.c               |   6 +-
 src/grippers.c             | 185 +++++++++++++++++++++++++------------
 src/motion_manager.c       |   2 +-
 src/ram.c                  |  12 ++-
 8 files changed, 176 insertions(+), 87 deletions(-)

diff --git a/include/darwin_registers.h b/include/darwin_registers.h
index c91a832..8b5c191 100644
--- a/include/darwin_registers.h
+++ b/include/darwin_registers.h
@@ -75,15 +75,17 @@ extern "C" {
 #define HEAD_TILT_I                     ((unsigned short int)0x004E)
 #define HEAD_TILT_D                     ((unsigned short int)0x0050)
 #define HEAD_TILT_I_CLAMP               ((unsigned short int)0x0052)
-#define GRIPPER_LEFT_ID                 ((unsigned short int)0x0054)
-#define GRIPPER_LEFT_MAX_ANGLE          ((unsigned short int)0x0055)
-#define GRIPPER_LEFT_MIN_ANGLE          ((unsigned short int)0x0057)
-#define GRIPPER_LEFT_MAX_FORCE          ((unsigned short int)0x0059)
-#define GRIPPER_RIGHT_ID                ((unsigned short int)0x005B)
-#define GRIPPER_RIGHT_MAX_ANGLE         ((unsigned short int)0x005C)
-#define GRIPPER_RIGHT_MIN_ANGLE         ((unsigned short int)0x005E)
-#define GRIPPER_RIGHT_MAX_FORCE         ((unsigned short int)0x0060)
-#define SMART_CHARGER_PERIOD            ((unsigned short int)0x0062)
+#define GRIPPER_LEFT_TOP_ID             ((unsigned short int)0x0054)
+#define GRIPPER_LEFT_BOT_ID             ((unsigned short int)0x0055)
+#define GRIPPER_LEFT_MAX_ANGLE          ((unsigned short int)0x0056)
+#define GRIPPER_LEFT_MIN_ANGLE          ((unsigned short int)0x0058)
+#define GRIPPER_LEFT_MAX_FORCE          ((unsigned short int)0x005A)
+#define GRIPPER_RIGHT_TOP_ID            ((unsigned short int)0x005C)
+#define GRIPPER_RIGHT_BOT_ID            ((unsigned short int)0x005D)
+#define GRIPPER_RIGHT_MAX_ANGLE         ((unsigned short int)0x005E)
+#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 LAST_EEPROM_OFFSET              ((unsigned short int)0x00FF)
 
@@ -172,14 +174,16 @@ typedef enum {
   DARWIN_HEAD_TILT_D_H                 = HEAD_TILT_D+1,
   DARWIN_HEAD_TILT_I_CLAMP_L           = HEAD_TILT_I_CLAMP,// max error in fixed point format 9|7 
   DARWIN_HEAD_TILT_I_CLAMP_H           = HEAD_TILT_I_CLAMP+1,
-  DARWIN_GRIPPER_LEFT_ID               = GRIPPER_LEFT_ID,
+  DARWIN_GRIPPER_LEFT_TOP_ID           = GRIPPER_LEFT_TOP_ID,
+  DARWIN_GRIPPER_LEFT_BOT_ID           = GRIPPER_LEFT_BOT_ID,
   DARWIN_GRIPPER_LEFT_MAX_ANGLE_L      = GRIPPER_LEFT_MAX_ANGLE,
   DARWIN_GRIPPER_LEFT_MAX_ANGLE_H      = GRIPPER_LEFT_MAX_ANGLE+1,
   DARWIN_GRIPPER_LEFT_MIN_ANGLE_L      = GRIPPER_LEFT_MIN_ANGLE,
   DARWIN_GRIPPER_LEFT_MIN_ANGLE_H      = GRIPPER_LEFT_MIN_ANGLE+1,
   DARWIN_GRIPPER_LEFT_MAX_FORCE_L      = GRIPPER_LEFT_MAX_FORCE,
   DARWIN_GRIPPER_LEFT_MAX_FORCE_H      = GRIPPER_LEFT_MAX_FORCE+1,
-  DARWIN_GRIPPER_RIGHT_ID              = GRIPPER_RIGHT_ID,
+  DARWIN_GRIPPER_RIGHT_TOP_ID          = GRIPPER_RIGHT_TOP_ID,
+  DARWIN_GRIPPER_RIGHT_BOT_ID          = GRIPPER_RIGHT_BOT_ID,
   DARWIN_GRIPPER_RIGHT_MAX_ANGLE_L     = GRIPPER_RIGHT_MAX_ANGLE,
   DARWIN_GRIPPER_RIGHT_MAX_ANGLE_H     = GRIPPER_RIGHT_MAX_ANGLE+1,
   DARWIN_GRIPPER_RIGHT_MIN_ANGLE_L     = GRIPPER_RIGHT_MIN_ANGLE,
@@ -580,8 +584,8 @@ typedef enum {
   DARWIN_SMART_CHARGER_BATT_STATUS_H     = 0x024F,
   DARWIN_SMART_CHARGER_CNTRL             = 0x0250, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 |    bit 1   |   bit 0
                                                    //                                               |  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 |       |       | close left | open left  | close right | open right 
+  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_registers;
 
 #define      GPIO_BASE_ADDRESS       0x0100
@@ -668,11 +672,15 @@ typedef enum {
 #define      GRIPPER_BASE_ADDRESS    0x0251
 #define      GRIPPER_MEM_LENGTH      1
 #define      GRIPPER_EEPROM_ADDRESS  0x0054
-#define      GRIPPER_EEPROM_LENGTH   14
+#define      GRIPPER_EEPROM_LENGTH   16
 #define      GRIPPER_OPEN_RIGHT      0x01
 #define      GRIPPER_CLOSE_RIGHT     0x02
 #define      GRIPPER_OPEN_LEFT       0x04
 #define      GRIPPER_CLOSE_LEFT      0x08
+#define      GRIPPER_MOVING_LEFT     0x20
+#define      GRIPPER_MOVING_RIGHT    0x10
+#define      GRIPPER_OPENED_LEFT     0x80
+#define      GRIPPER_OPENED_RIGHT    0x40
 
 #define      SMART_CHARGER_BASE_ADDRESS    0x0247
 #define      SMART_CHARGER_MEM_LENGTH      11
diff --git a/include/eeprom_init.h b/include/eeprom_init.h
index dafe223..e7ae5eb 100644
--- a/include/eeprom_init.h
+++ b/include/eeprom_init.h
@@ -73,11 +73,13 @@ extern "C" {
 #define    DEFAULT_HEAD_TILT_I                  0x0000 // 0.0 in fixed point format 0|16
 #define    DEFAULT_HEAD_TILT_D                  0x0000 // 0.0 in fixed point format 0|16
 #define    DEFAULT_HEAD_TILT_I_CLAMP            0x0000 // 0.0 in fixed point format 9|7
-#define    DEFAULT_GRIPPER_LEFT_ID              0x0017 // ID 23 for the left gripper
+#define    DEFAULT_GRIPPER_LEFT_TOP_ID          0x0015 // ID 21 for the left gripper
+#define    DEFAULT_GRIPPER_LEFT_BOT_ID          0x0016 // ID 22 for the left gripper
 #define    DEFAULT_GRIPPER_LEFT_MAX_ANGLE       0x0F00 // 30 in fixed point format 9|7
 #define    DEFAULT_GRIPPER_LEFT_MIN_ANGLE       0xF100 // -30 in fixed point format 9|7
 #define    DEFAULT_GRIPPER_LEFT_MAX_FORCE       0x03FF // 1023 max force in binary format
-#define    DEFAULT_GRIPPER_RIGHT_ID             0x0018 // ID 24 for the left gripper
+#define    DEFAULT_GRIPPER_RIGHT_TOP_ID         0x0017 // ID 23 for the left gripper
+#define    DEFAULT_GRIPPER_RIGHT_BOT_ID         0x0018 // ID 24 for the left gripper
 #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      0x03FF // 1023 max force in binary format
diff --git a/include/grippers.h b/include/grippers.h
index 9df31bc..9a1aa5f 100644
--- a/include/grippers.h
+++ b/include/grippers.h
@@ -16,13 +16,15 @@ inline void grippers_set_period(uint16_t period_us);
 inline uint16_t grippers_get_period(void);
 void grippers_open(grippers_t gripper);
 void grippers_close(grippers_t gripper);
+void grippers_stop(grippers_t gripper);
 uint8_t gripper_is_open(grippers_t gripper);
+uint8_t gripper_is_close(grippers_t gripper);
+uint8_t gripper_is_moving(grippers_t gripper);
 
 // operation functions
 uint8_t grippers_in_range(unsigned short int address, unsigned short int length);
 void grippers_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
 void grippers_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
-// motion manager interface functions
 void grippers_process(void);
 
 #ifdef __cplusplus
diff --git a/include/motion_manager.h b/include/motion_manager.h
index 641c39d..d0f234c 100755
--- a/include/motion_manager.h
+++ b/include/motion_manager.h
@@ -29,7 +29,11 @@ typedef enum {
   R_ANKLE_ROLL         = 17,
   L_ANKLE_ROLL         = 18,
   L_PAN                = 19,
-  L_TILT               = 20} servo_id_t;
+  L_TILT               = 20,
+  L_GRIPPER_TOP        = 21,
+  L_GRIPPER_BOT        = 22,
+  R_GRIPPER_TOP        = 23,
+  R_GRIPPER_BOT        = 24} servo_id_t;
 
 
 typedef enum {MM_NONE          = 0,
@@ -38,9 +42,9 @@ typedef enum {MM_NONE          = 0,
               MM_JOINTS        = 3,
               MM_HEAD          = 4} TModules;
 
-typedef enum {MM_FWD_FALL      =0,
-              MM_BWD_FALL      =1, 
-              MM_STANDING      =2} TFall;
+typedef enum {MM_FWD_FALL      = 0,
+              MM_BWD_FALL      = 1, 
+              MM_STANDING      = 2} TFall;
 
 typedef struct
 {
diff --git a/src/eeprom.c b/src/eeprom.c
index 4fa6f8e..fbb5a1a 100755
--- a/src/eeprom.c
+++ b/src/eeprom.c
@@ -139,14 +139,16 @@ uint16_t eeprom_data[] __attribute__ ((section (".eeprom")))={VALID_PAGE,
                                                               DEFAULT_HEAD_TILT_D>>8,                    HEAD_TILT_D+1,
                                                               DEFAULT_HEAD_TILT_I_CLAMP&0xFF,            HEAD_TILT_I_CLAMP,
                                                               DEFAULT_HEAD_TILT_I_CLAMP>>8,              HEAD_TILT_I_CLAMP+1,
-                                                              DEFAULT_GRIPPER_LEFT_ID,                   GRIPPER_LEFT_ID,
+                                                              DEFAULT_GRIPPER_LEFT_TOP_ID,               GRIPPER_LEFT_TOP_ID,
+                                                              DEFAULT_GRIPPER_LEFT_BOT_ID,               GRIPPER_LEFT_BOT_ID,
                                                               DEFAULT_GRIPPER_LEFT_MAX_ANGLE&0xFF,       GRIPPER_LEFT_MAX_ANGLE,
                                                               DEFAULT_GRIPPER_LEFT_MAX_ANGLE>>8,         GRIPPER_LEFT_MAX_ANGLE+1,
                                                               DEFAULT_GRIPPER_LEFT_MIN_ANGLE&0xFF,       GRIPPER_LEFT_MIN_ANGLE,
                                                               DEFAULT_GRIPPER_LEFT_MIN_ANGLE>>8,         GRIPPER_LEFT_MIN_ANGLE+1,
                                                               DEFAULT_GRIPPER_LEFT_MAX_FORCE&0xFF,       GRIPPER_LEFT_MAX_FORCE,
                                                               DEFAULT_GRIPPER_LEFT_MAX_FORCE>>8,         GRIPPER_LEFT_MAX_FORCE+1,
-                                                              DEFAULT_GRIPPER_RIGHT_ID,                  GRIPPER_RIGHT_ID,
+                                                              DEFAULT_GRIPPER_RIGHT_TOP_ID,              GRIPPER_RIGHT_TOP_ID,
+                                                              DEFAULT_GRIPPER_RIGHT_BOT_ID,              GRIPPER_RIGHT_BOT_ID,
                                                               DEFAULT_GRIPPER_RIGHT_MAX_ANGLE&0xFF,      GRIPPER_RIGHT_MAX_ANGLE,
                                                               DEFAULT_GRIPPER_RIGHT_MAX_ANGLE>>8,        GRIPPER_RIGHT_MAX_ANGLE+1,
                                                               DEFAULT_GRIPPER_RIGHT_MIN_ANGLE&0xFF,      GRIPPER_RIGHT_MIN_ANGLE,
diff --git a/src/grippers.c b/src/grippers.c
index c5fae08..5544346 100644
--- a/src/grippers.c
+++ b/src/grippers.c
@@ -2,42 +2,48 @@
 #include "dyn_servos.h"
 #include "grippers.h"
 #include "ram.h"
+#include "joint_motion.h"
 
 // private variables
 int64_t grippers_period;
 int16_t grippers_period_us;
-uint8_t grippers_left_opened;
-uint8_t grippers_left_open;
-uint8_t grippers_left_close;
-uint8_t grippers_right_opened;
-uint8_t grippers_right_open;
-uint8_t grippers_right_close;
+uint8_t gripper_left_target;
+uint8_t gripper_right_target;
 
 // public functions
 void grippers_init(uint16_t period_us)
 {
   uint16_t force;
+  uint32_t servo_mask=0x00000000;
 
   /* initialize private variables */
-  grippers_left_opened=0x00;
-  grippers_left_open=0x00;
-  grippers_left_close=0x00;
-  grippers_right_opened=0x00;
-  grippers_right_open=0x00;
-  grippers_right_close=0x00;
+  gripper_left_target=0x00;//close;
+  gripper_right_target=0x00;//close;
   grippers_period=(period_us<<16)/1000000;
   grippers_period_us=period_us;
   /* configure the maximum force of the servos */
-  if(ram_data[DARWIN_GRIPPER_LEFT_ID]!=0xFF)
+  servo_mask=0x00000000;
+  if(ram_data[DARWIN_GRIPPER_LEFT_TOP_ID]!=0xFF && ram_data[DARWIN_GRIPPER_LEFT_BOT_ID]!=0xFF)
   {
     force=ram_data[DARWIN_GRIPPER_LEFT_MAX_FORCE_L]+(ram_data[DARWIN_GRIPPER_LEFT_MAX_FORCE_H]<<8);
-    dyn_master_write_word(&darwin_dyn_master_v2,ram_data[DARWIN_GRIPPER_LEFT_ID],XL_TORQUE_LIMIT_L,force);
+    dyn_master_write_word(&darwin_dyn_master_v2,ram_data[DARWIN_GRIPPER_LEFT_TOP_ID],XL_TORQUE_LIMIT_L,force);
+    dyn_master_write_word(&darwin_dyn_master_v2,ram_data[DARWIN_GRIPPER_LEFT_BOT_ID],XL_TORQUE_LIMIT_L,force);
+    servo_mask|=1<<ram_data[DARWIN_GRIPPER_LEFT_TOP_ID];
+    servo_mask|=1<<ram_data[DARWIN_GRIPPER_LEFT_BOT_ID];
+    joint_motion_set_group_servos(1,servo_mask);
   }
-  if(ram_data[DARWIN_GRIPPER_RIGHT_ID]!=0xFF)
+  servo_mask=0x00000000;
+  if(ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID]!=0xFF && ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID]!=0xFF)
   {
     force=ram_data[DARWIN_GRIPPER_RIGHT_MAX_FORCE_L]+(ram_data[DARWIN_GRIPPER_RIGHT_MAX_FORCE_H]<<8);
-    dyn_master_write_word(&darwin_dyn_master_v2,ram_data[DARWIN_GRIPPER_RIGHT_ID],XL_TORQUE_LIMIT_L,force);
+    dyn_master_write_word(&darwin_dyn_master_v2,ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID],XL_TORQUE_LIMIT_L,force);
+    dyn_master_write_word(&darwin_dyn_master_v2,ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID],XL_TORQUE_LIMIT_L,force);
+    servo_mask|=1<<ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID];
+    servo_mask|=1<<ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID];
+    joint_motion_set_group_servos(2,servo_mask);
   }
+  grippers_close(GRIPPER_LEFT);
+  grippers_close(GRIPPER_RIGHT);
 }
 
 inline void grippers_set_period(uint16_t period_us)
@@ -54,27 +60,101 @@ inline uint16_t grippers_get_period(void)
 void grippers_open(grippers_t gripper)
 {
   if(gripper==GRIPPER_LEFT)
-    grippers_left_open=0x01;
+  {
+    ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_TOP_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_L];
+    ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_TOP_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_H];
+    ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_LEFT_TOP_ID]]=60;
+    ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_BOT_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_L];
+    ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_BOT_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_H];
+    ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_LEFT_BOT_ID]]=60;
+    joint_motion_start(1);
+  }
   else if(gripper==GRIPPER_RIGHT)
-    grippers_right_open=0x01;
+  {
+    ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_L];
+    ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_H];
+    ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID]]=60;
+    ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_L];
+    ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_H];
+    ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID]]=60;
+    joint_motion_start(2);
+  }
 }
 
 void grippers_close(grippers_t gripper)
 {
   if(gripper==GRIPPER_LEFT)
-    grippers_left_close=0x01;
+  {
+    ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_TOP_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_L];
+    ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_TOP_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_H];
+    ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_LEFT_TOP_ID]]=60;
+    ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_BOT_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_L];
+    ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_BOT_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_H];
+    ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_LEFT_BOT_ID]]=60;
+    joint_motion_start(1);
+  }
   else if(gripper==GRIPPER_RIGHT)
-    grippers_right_close=0x01;
+  {
+    ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_L];
+    ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_H];
+    ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID]]=60;
+    ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_L];
+    ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_H];
+    ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID]]=60;
+    joint_motion_start(2);
+  }
+}
+
+void grippers_stop(grippers_t gripper)
+{
+  if(gripper==GRIPPER_LEFT)
+    joint_motion_stop(1);
+  else
+    joint_motion_stop(2);
 }
 
 uint8_t gripper_is_open(grippers_t gripper)
 {
   if(gripper==GRIPPER_LEFT)
-    return grippers_left_opened;
-  else if(gripper==GRIPPER_RIGHT)
-    return grippers_right_opened;
+  {
+    if(ram_data[DARWIN_GRIPPER_CNTRL]&GRIPPER_OPENED_LEFT)
+      return 0x01;
+    else
+      return 0x00;
+  }
   else
-    return 0x00;
+  {
+    if(ram_data[DARWIN_GRIPPER_CNTRL]&GRIPPER_OPENED_RIGHT)
+      return 0x01;
+    else
+      return 0x00;
+  }
+}
+
+uint8_t gripper_is_close(grippers_t gripper)
+{
+  if(gripper==GRIPPER_LEFT)
+  {
+    if(ram_data[DARWIN_GRIPPER_CNTRL]&GRIPPER_OPENED_LEFT)
+      return 0x00;
+    else
+      return 0x01;
+  }
+  else
+  {
+    if(ram_data[DARWIN_GRIPPER_CNTRL]&GRIPPER_OPENED_RIGHT)
+      return 0x00;
+    else
+      return 0x01;
+  }
+}
+
+uint8_t gripper_is_moving(grippers_t gripper)
+{
+  if(gripper==GRIPPER_LEFT)
+    return are_joints_moving(1);
+  else
+    return are_joints_moving(2);
 }
 
 // operation functions
@@ -107,45 +187,32 @@ void grippers_process_write_cmd(unsigned short int address,unsigned short int le
   }
 }
 
-// motion manager interface functions
 void grippers_process(void)
 {
-  uint16_t angle;
-
-  if(ram_data[DARWIN_GRIPPER_LEFT_ID]!=0xFF)
+  if(!are_joints_moving(1))
+  {
+    ram_data[DARWIN_GRIPPER_CNTRL]&=~GRIPPER_MOVING_LEFT;
+    if(gripper_left_target==0x01)
+      ram_data[DARWIN_GRIPPER_CNTRL]|=GRIPPER_OPENED_LEFT;
+    else
+      ram_data[DARWIN_GRIPPER_CNTRL]&=~GRIPPER_OPENED_LEFT;
+  }
+  else
   {
-    if(grippers_left_opened==0x01)
-    {
-      if(grippers_left_close==0x01)
-      {
-        grippers_left_close=0x00;
-        angle=ram_data[DARWIN_GRIPPER_LEFT_MIN_ANGLE_L]+(ram_data[DARWIN_GRIPPER_LEFT_MIN_ANGLE_H]<<8);
-        manager_current_angles[ram_data[DARWIN_GRIPPER_LEFT_ID]]=angle<<9;
-      }
-      else if(grippers_left_open==0x01)
-      {
-        grippers_left_open=0x00;
-        angle=ram_data[DARWIN_GRIPPER_LEFT_MAX_ANGLE_L]+(ram_data[DARWIN_GRIPPER_LEFT_MAX_ANGLE_H]<<8);
-        manager_current_angles[ram_data[DARWIN_GRIPPER_LEFT_ID]]=angle<<9;
-      }
-    }
+    ram_data[DARWIN_GRIPPER_CNTRL]&=~GRIPPER_OPENED_LEFT;
+    ram_data[DARWIN_GRIPPER_CNTRL]|=GRIPPER_MOVING_LEFT;
   }
-  if(ram_data[DARWIN_GRIPPER_RIGHT_ID]!=0xFF)
+  if(!are_joints_moving(2))
+  {
+    ram_data[DARWIN_GRIPPER_CNTRL]&=~GRIPPER_MOVING_RIGHT;
+    if(gripper_right_target==0x01)
+      ram_data[DARWIN_GRIPPER_CNTRL]|=GRIPPER_OPENED_RIGHT;
+    else
+      ram_data[DARWIN_GRIPPER_CNTRL]&=~GRIPPER_OPENED_RIGHT;
+  }
+  else
   {
-    if(grippers_right_opened==0x01)
-    {
-      if(grippers_right_close==0x01)
-      {
-        grippers_right_close=0x00;
-        angle=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_L]+(ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_H]<<8);
-        manager_current_angles[ram_data[DARWIN_GRIPPER_RIGHT_ID]]=angle<<9;
-      }
-      else if(grippers_right_open==0x01)
-      {
-        grippers_right_open=0x00;
-        angle=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_L]+(ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_H]<<8);
-        manager_current_angles[ram_data[DARWIN_GRIPPER_RIGHT_ID]]=angle<<9;
-      }
-    }
+    ram_data[DARWIN_GRIPPER_CNTRL]&=~GRIPPER_OPENED_RIGHT;
+    ram_data[DARWIN_GRIPPER_CNTRL]|=GRIPPER_MOVING_RIGHT;
   }
 }
diff --git a/src/motion_manager.c b/src/motion_manager.c
index b6bb218..65f06bf 100755
--- a/src/motion_manager.c
+++ b/src/motion_manager.c
@@ -196,7 +196,7 @@ void MANAGER_TIMER_IRQHandler(void)
       head_tracking_process();
       // call the walking process
       walking_process();
-      // call the grippers process
+      // call the gripper process
       grippers_process();
       // balance the robot
       manager_balance(); 
diff --git a/src/ram.c b/src/ram.c
index e061c05..e708a46 100755
--- a/src/ram.c
+++ b/src/ram.c
@@ -119,8 +119,10 @@ void ram_init(void)
     ram_data[HEAD_TILT_I_CLAMP]=(uint8_t)(eeprom_data&0x00FF);
   if(EE_ReadVariable(HEAD_TILT_I_CLAMP+1,&eeprom_data)==0)
     ram_data[HEAD_TILT_I_CLAMP+1]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(GRIPPER_LEFT_ID,&eeprom_data)==0)
-    ram_data[GRIPPER_LEFT_ID]=(uint8_t)(eeprom_data&0x00FF);
+  if(EE_ReadVariable(GRIPPER_LEFT_TOP_ID,&eeprom_data)==0)
+    ram_data[GRIPPER_LEFT_TOP_ID]=(uint8_t)(eeprom_data&0x00FF);
+  if(EE_ReadVariable(GRIPPER_LEFT_BOT_ID,&eeprom_data)==0)
+    ram_data[GRIPPER_LEFT_BOT_ID]=(uint8_t)(eeprom_data&0x00FF);
   if(EE_ReadVariable(GRIPPER_LEFT_MAX_ANGLE,&eeprom_data)==0)
     ram_data[GRIPPER_LEFT_MAX_ANGLE]=(uint8_t)(eeprom_data&0x00FF);
   if(EE_ReadVariable(GRIPPER_LEFT_MAX_ANGLE+1,&eeprom_data)==0)
@@ -133,8 +135,10 @@ void ram_init(void)
     ram_data[GRIPPER_LEFT_MAX_FORCE]=(uint8_t)(eeprom_data&0x00FF);
   if(EE_ReadVariable(GRIPPER_LEFT_MAX_FORCE+1,&eeprom_data)==0)
     ram_data[GRIPPER_LEFT_MAX_FORCE+1]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(GRIPPER_RIGHT_ID,&eeprom_data)==0)
-    ram_data[GRIPPER_RIGHT_ID]=(uint8_t)(eeprom_data&0x00FF);
+  if(EE_ReadVariable(GRIPPER_RIGHT_TOP_ID,&eeprom_data)==0)
+    ram_data[GRIPPER_RIGHT_TOP_ID]=(uint8_t)(eeprom_data&0x00FF);
+  if(EE_ReadVariable(GRIPPER_RIGHT_BOT_ID,&eeprom_data)==0)
+    ram_data[GRIPPER_RIGHT_BOT_ID]=(uint8_t)(eeprom_data&0x00FF);
   if(EE_ReadVariable(GRIPPER_RIGHT_MAX_ANGLE,&eeprom_data)==0)
     ram_data[GRIPPER_RIGHT_MAX_ANGLE]=(uint8_t)(eeprom_data&0x00FF);
   if(EE_ReadVariable(GRIPPER_RIGHT_MAX_ANGLE+1,&eeprom_data)==0)
-- 
GitLab


From 58aa3e4e745a0f91a20537dd282d993c46352a67 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Fri, 30 Jun 2017 12:52:20 +0200
Subject: [PATCH 05/21] Set the stop signal to 0 when a new motion is started
 to avoid false cancelations. Added a new variable to hold the angle required
 by the deacceleration phase with the initial command data, instead of
 computing it each cycle. Changed the max force default value of the grippers.

---
 include/eeprom_init.h | 4 ++--
 src/joint_motion.c    | 8 +++++++-
 2 files changed, 9 insertions(+), 3 deletions(-)

diff --git a/include/eeprom_init.h b/include/eeprom_init.h
index e7ae5eb..cb4543c 100644
--- a/include/eeprom_init.h
+++ b/include/eeprom_init.h
@@ -77,12 +77,12 @@ extern "C" {
 #define    DEFAULT_GRIPPER_LEFT_BOT_ID          0x0016 // ID 22 for the left gripper
 #define    DEFAULT_GRIPPER_LEFT_MAX_ANGLE       0x0F00 // 30 in fixed point format 9|7
 #define    DEFAULT_GRIPPER_LEFT_MIN_ANGLE       0xF100 // -30 in fixed point format 9|7
-#define    DEFAULT_GRIPPER_LEFT_MAX_FORCE       0x03FF // 1023 max force in binary format
+#define    DEFAULT_GRIPPER_LEFT_MAX_FORCE       0x0080 // 1023 max force in binary format
 #define    DEFAULT_GRIPPER_RIGHT_TOP_ID         0x0017 // ID 23 for the left gripper
 #define    DEFAULT_GRIPPER_RIGHT_BOT_ID         0x0018 // ID 24 for the left gripper
 #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      0x03FF // 1023 max force in binary format
+#define    DEFAULT_GRIPPER_RIGHT_MAX_FORCE      0x0080 // 1023 max force in binary format
 #define    DEFAULT_SMART_CHARGER_PERIOD         0x05DC //1500 ms 
 
   
diff --git a/src/joint_motion.c b/src/joint_motion.c
index 8e8147a..395bfc0 100644
--- a/src/joint_motion.c
+++ b/src/joint_motion.c
@@ -6,6 +6,7 @@
 int64_t joint_target_angles[MANAGER_MAX_NUM_SERVOS];//48|16
 int64_t joint_target_speeds[MANAGER_MAX_NUM_SERVOS];//48|16
 int64_t joint_target_accels[MANAGER_MAX_NUM_SERVOS];//48|16
+int64_t joint_target_stop_angles[MANAGER_MAX_NUM_SERVOS];//48|16
 
 int64_t joint_current_angles[MANAGER_MAX_NUM_SERVOS];//48|16
 int64_t joint_current_speeds[MANAGER_MAX_NUM_SERVOS];//48|16
@@ -120,6 +121,8 @@ void joint_motion_start(uint8_t group_id)
             ram_data[DARWIN_JOINT_SERVO0_ACCEL+i]=joint_target_accels[i]>>16;
           }
         }
+        // stop angles
+        joint_target_stop_angles[i]=joint_target_speeds[i]*joint_target_speeds[i]/(2*joint_target_accels[i]);
         // the current angles, speeds and accelerations are in RAM
         if(joint_target_angles[i]>=joint_current_angles[i])
           joint_dir[i]=1;
@@ -129,6 +132,7 @@ void joint_motion_start(uint8_t group_id)
     }
   }
   joint_moving[group_id]=0x01;
+  joint_stop[group_id]=0x00;
   ram_data[DARWIN_JOINT_GRP0_CNTRL+group_id*5]|=JOINT_STATUS;
 }
 
@@ -236,7 +240,7 @@ void joint_motion_process(void)
               }  
               else
               {
-                if(abs(joint_target_angles[i]-joint_current_angles[i])>(joint_target_speeds[i]*joint_target_speeds[i]/(2*joint_target_accels[i])))// constant speed phase
+                if(abs(joint_target_angles[i]-joint_current_angles[i])>joint_target_stop_angles[i])// constant speed phase
                 {
                   joint_current_angles[i]+=((joint_current_speeds[i]*joint_motion_period)>>16);
                   if(joint_dir[i]==1 && joint_current_angles[i]>joint_target_angles[i])
@@ -256,6 +260,8 @@ void joint_motion_process(void)
                 }
               }
             }
+            else
+              joint_current_speeds[i]=0;
             manager_current_angles[i]=joint_current_angles[i];
             manager_current_slopes[i]=5;
           }
-- 
GitLab


From 637a115e3ced62157bd81668ba440ca8ab559b11 Mon Sep 17 00:00:00 2001
From: darwin <darwin@darwin.users.iri.prv>
Date: Fri, 25 Aug 2017 00:02:01 +0200
Subject: [PATCH 06/21] Solved a problem with the sign of the accelerometer
 sensor. Chnaged the sign of the X axis of the gyroscope sensor.

---
 src/imu.c            | 2 +-
 src/motion_manager.c | 8 ++++----
 2 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/src/imu.c b/src/imu.c
index e792585..bbaed72 100755
--- a/src/imu.c
+++ b/src/imu.c
@@ -258,7 +258,7 @@ void imu_accel_convert_data(uint8_t *data_in,uint8_t *data_out)
 
   for(i=0;i<3;i++)
   {
-    accel_data[i]=(data_in[i*2]+(data_in[i*2+1]<<8));
+    accel_data[i]=(int16_t)(data_in[i*2]+(data_in[i*2+1]<<8));
     data_out[i*2]=accel_data[i]%256;
     data_out[i*2+1]=accel_data[i]/256;
   }
diff --git a/src/motion_manager.c b/src/motion_manager.c
index 65f06bf..8faa546 100755
--- a/src/motion_manager.c
+++ b/src/motion_manager.c
@@ -149,10 +149,10 @@ void manager_balance(void)
     manager_balance_offset[R_ANKLE_PITCH]=((((int64_t)gyro_y*(int64_t)ankle_pitch_gain)/12000)>>9);    
     manager_balance_offset[L_KNEE]=((((int64_t)gyro_y*(int64_t)knee_gain)/12000)>>9);    
     manager_balance_offset[L_ANKLE_PITCH]=-((((int64_t)gyro_y*(int64_t)ankle_pitch_gain)/12000)>>9);
-    manager_balance_offset[R_HIP_ROLL]=-((((int64_t)gyro_x*(int64_t)hip_roll_gain)/12000)>>9);
-    manager_balance_offset[L_HIP_ROLL]=-((((int64_t)gyro_x*(int64_t)hip_roll_gain)/12000)>>9);
-    manager_balance_offset[R_ANKLE_ROLL]=((((int64_t)gyro_x*(int64_t)ankle_roll_gain)/12000)>>9);
-    manager_balance_offset[L_ANKLE_ROLL]=((((int64_t)gyro_x*(int64_t)ankle_roll_gain)/12000)>>9);
+    manager_balance_offset[R_HIP_ROLL]=((((int64_t)gyro_x*(int64_t)hip_roll_gain)/12000)>>9);
+    manager_balance_offset[L_HIP_ROLL]=((((int64_t)gyro_x*(int64_t)hip_roll_gain)/12000)>>9);
+    manager_balance_offset[R_ANKLE_ROLL]=-((((int64_t)gyro_x*(int64_t)ankle_roll_gain)/12000)>>9);
+    manager_balance_offset[L_ANKLE_ROLL]=-((((int64_t)gyro_x*(int64_t)ankle_roll_gain)/12000)>>9);
   }
   // fall detection (using accelerometer)
   imu_get_accel_data(&accel_x,&accel_y,&accel_z);
-- 
GitLab


From 54d8e70eac35249c54e6101b0d77ffe0ec573913 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Fri, 25 Aug 2017 12:52:45 +0200
Subject: [PATCH 07/21] Changed one analog port to a digital output to control
 the power on the second dynamixel bus.

---
 include/adc_dma.h              |  2 +-
 include/darwin_dyn_master_v2.h |  2 ++
 include/darwin_registers.h     |  9 +++-----
 include/smart_charger.h        |  2 +-
 src/adc_dma.c                  | 38 +++++++++-------------------------
 src/darwin_dyn_master_v2.c     | 16 ++++++++++++++
 src/motion_manager.c           |  4 ++++
 7 files changed, 37 insertions(+), 36 deletions(-)

diff --git a/include/adc_dma.h b/include/adc_dma.h
index 0fd1dc4..db29c8d 100755
--- a/include/adc_dma.h
+++ b/include/adc_dma.h
@@ -10,7 +10,7 @@ extern "C" {
 #define      ADC_NUM_CHANNELS      12
 
 typedef enum {ADC_CH1=0,ADC_CH2=1,ADC_CH3=2,ADC_CH4=3,ADC_CH5=4,ADC_CH6=5,ADC_CH7=6,ADC_CH8=7,
-              ADC_CH9=8,ADC_CH10=9,ADC_CH12=10,ADC_CH14=11} adc_ch_t;
+              ADC_CH9=8,ADC_CH10=9,ADC_CH12=10} adc_ch_t;
 
 void adc_init(void);
 void adc_start(void);
diff --git a/include/darwin_dyn_master_v2.h b/include/darwin_dyn_master_v2.h
index b4a1b34..a7e5d19 100644
--- a/include/darwin_dyn_master_v2.h
+++ b/include/darwin_dyn_master_v2.h
@@ -12,6 +12,8 @@ extern "C" {
 extern TDynamixelMaster darwin_dyn_master_v2;
 
 void darwin_dyn_master_v2_init(void);
+inline void darwin_dyn_master_v2_enable_power(void);
+inline void darwin_dyn_master_v2_disable_power(void);
 
 #ifdef __cplusplus
 }
diff --git a/include/darwin_registers.h b/include/darwin_registers.h
index 8b5c191..6cf97e2 100644
--- a/include/darwin_registers.h
+++ b/include/darwin_registers.h
@@ -254,10 +254,6 @@ typedef enum {
   DARWIN_ADC_TEMP_H                = 0x012C,
   DARWIN_ADC_CH12_L                = 0x012D,
   DARWIN_ADC_CH12_H                = 0x012E,
-  DARWIN_ADC_VREF_L                = 0x012F,
-  DARWIN_ADC_VREF_H                = 0x0130,
-  DARWIN_ADC_CH14_L                = 0x0131,
-  DARWIN_ADC_CH14_H                = 0x0132,
   DARWIN_IMU_CNTRL                 = 0x013B, //   bit 7   |   bit 6  |    bit 5    |  bit 4  | bit 3 |    bit 2   | bit 1 | bit 0
                                              // accel_det | gyro_det | calibrating | running |       | start_cal  | stop  | start
   DARWIN_IMU_CAL_SAMPLES_L         = 0x013C,
@@ -276,8 +272,8 @@ typedef enum {
   DARWIN_IMU_ACCEL_Z_H             = 0x0149,
 
   DARWIN_MM_NUM_SERVOS             = 0x014A,
-  DARWIN_MM_CNTRL                  = 0x014B, //   bit 7  |   bit 6  |   bit 5  | bit 4 | bit 3 |     bit 2    |      bit 1      |    bit 0
-                                             // scanning | fwd fall | bwd fall |       |       | Enable power | Enable balance  | Enable manager
+  DARWIN_MM_CNTRL                  = 0x014B, //   bit 7  |   bit 6  |   bit 5  | bit 4 |       bit 3      |     bit 2    |      bit 1      |    bit 0
+                                             // scanning | fwd fall | bwd fall |       | Enable power v2  | Enable power | Enable balance  | Enable manager
   DARWIN_MM_MODULE_EN0             = 0x014C, //      bit 7      | bit 6 | bit 5 | bit 4 |      bit 3      | bit 2 | bit 1 | bit 0i
                                              // Enable servo 0  |   assigned module     | Enable servo 1  |   assigned module
                                              //                 |     000 -> none       |
@@ -630,6 +626,7 @@ typedef enum {
 #define      MANAGER_ENABLE          0x01
 #define      MANAGER_EN_BAL          0x02
 #define      MANAGER_EN_PWR          0x04
+#define      MANAGER_EN_PWR_V2       0x08
 #define      MANAGER_SCANNING        0x80
 #define      MANAGER_FWD_FALL        0x40
 #define      MANAGER_BWD_FALL        0x20
diff --git a/include/smart_charger.h b/include/smart_charger.h
index 5d3587c..42c3217 100644
--- a/include/smart_charger.h
+++ b/include/smart_charger.h
@@ -118,4 +118,4 @@ void smart_charger_process(void);
 }
 #endif
 
-#endif
\ No newline at end of file
+#endif
diff --git a/src/adc_dma.c b/src/adc_dma.c
index d24f466..eb0f821 100755
--- a/src/adc_dma.c
+++ b/src/adc_dma.c
@@ -7,15 +7,13 @@
 #define ADC1_CH4                  ADC_CHANNEL_7
 #define ADC1_CH5                  ADC_CHANNEL_8
 #define ADC1_CH6                  ADC_CHANNEL_TEMPSENSOR
-#define ADC1_CH7                  ADC_CHANNEL_VREFINT
 
 #define ADC2_CH1                  ADC_CHANNEL_9
 #define ADC2_CH2                  ADC_CHANNEL_10
 #define ADC2_CH3                  ADC_CHANNEL_11
 #define ADC2_CH4                  ADC_CHANNEL_12
-#define ADC2_CH5                  ADC_CHANNEL_13
-#define ADC2_CH6                  ADC_CHANNEL_14
-#define ADC2_CH7                  ADC_CHANNEL_15
+#define ADC2_CH5                  ADC_CHANNEL_14
+#define ADC2_CH6                  ADC_CHANNEL_15
   
 #define ADC1_CH1_PIN              GPIO_PIN_0
 #define ADC1_CH1_PORT             GPIOA
@@ -47,15 +45,12 @@
 #define ADC2_CH4_PIN              GPIO_PIN_2
 #define ADC2_CH4_PORT             GPIOC
 #define ADC2_CH4_ENABLE_PORT_CLK  __HAL_RCC_GPIOC_CLK_ENABLE()
-#define ADC2_CH5_PIN              GPIO_PIN_3
+#define ADC2_CH5_PIN              GPIO_PIN_4
 #define ADC2_CH5_PORT             GPIOC
 #define ADC2_CH5_ENABLE_PORT_CLK  __HAL_RCC_GPIOC_CLK_ENABLE()
-#define ADC2_CH6_PIN              GPIO_PIN_4
+#define ADC2_CH6_PIN              GPIO_PIN_5
 #define ADC2_CH6_PORT             GPIOC
 #define ADC2_CH6_ENABLE_PORT_CLK  __HAL_RCC_GPIOC_CLK_ENABLE()
-#define ADC2_CH7_PIN              GPIO_PIN_5
-#define ADC2_CH7_PORT             GPIOC
-#define ADC2_CH7_ENABLE_PORT_CLK  __HAL_RCC_GPIOC_CLK_ENABLE()
 
 #define ADC2_ENABLE_CLK           __ADC2_CLK_ENABLE()
 
@@ -82,7 +77,7 @@ ADC_HandleTypeDef hadc2;
 DMA_HandleTypeDef hdma_adc1; 
 TIM_HandleTypeDef ADC_TIM_Handle;
 
-uint32_t adc_data[9];// temporal buffer to store ADC data before conversion
+uint32_t adc_data[6];// temporal buffer to store ADC data before conversion
 uint16_t adc_period_ms;
 
 // private functions
@@ -153,7 +148,7 @@ void ADC_DMA_IRQHandler(void)
     if(__HAL_DMA_GET_IT_SOURCE(&hdma_adc1, DMA_IT_TC) != RESET)
     {
       __HAL_DMA_CLEAR_FLAG(&hdma_adc1, __HAL_DMA_GET_TC_FLAG_INDEX(&hdma_adc1));
-      for(i=0;i<7;i++)
+      for(i=0;i<6;i++)
       {
         if(i==5)
         {
@@ -202,7 +197,6 @@ void adc_init(void)
   ADC2_CH4_ENABLE_PORT_CLK;
   ADC2_CH5_ENABLE_PORT_CLK;
   ADC2_CH6_ENABLE_PORT_CLK;
-  ADC2_CH7_ENABLE_PORT_CLK;
 
   ADC_ENABLE_DMA_CLK;
 
@@ -214,7 +208,7 @@ void adc_init(void)
   hadc1.Init.DiscontinuousConvMode = DISABLE;
   hadc1.Init.ExternalTrigConv = ADC_EXTERNALTRIGCONV_T1_CC1;
   hadc1.Init.DataAlign = ADC_DATAALIGN_RIGHT;
-  hadc1.Init.NbrOfConversion = 7;
+  hadc1.Init.NbrOfConversion = 6;
   HAL_ADC_Init(&hadc1);
 
   multimode.Mode = ADC_DUALMODE_REGSIMULT;
@@ -246,10 +240,6 @@ void adc_init(void)
   sConfig.Rank = 6;
   HAL_ADC_ConfigChannel(&hadc1, &sConfig);
 
-  sConfig.Channel = ADC1_CH7;
-  sConfig.Rank = 7;
-  HAL_ADC_ConfigChannel(&hadc1, &sConfig);
-
   // configure GPIO 
   GPIO_InitStruct.Pin = ADC1_CH1_PIN|ADC1_CH2_PIN|ADC1_CH3_PIN|ADC1_CH4_PIN;
   GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
@@ -266,7 +256,7 @@ void adc_init(void)
   hadc2.Init.DiscontinuousConvMode = DISABLE;
   hadc2.Init.ExternalTrigConv = ADC_SOFTWARE_START;//ADC_EXTERNALTRIGCONV_T1_CC1;
   hadc2.Init.DataAlign = ADC_DATAALIGN_RIGHT;
-  hadc2.Init.NbrOfConversion = 7;
+  hadc2.Init.NbrOfConversion = 6;
   HAL_ADC_Init(&hadc2);
 
   // configure ADC2 channels 
@@ -295,13 +285,9 @@ void adc_init(void)
   sConfig.Rank = 6;
   HAL_ADC_ConfigChannel(&hadc2, &sConfig);
 
-  sConfig.Channel = ADC2_CH7;
-  sConfig.Rank = 7;
-  HAL_ADC_ConfigChannel(&hadc2, &sConfig);
-
   // configure GPIO 
   GPIO_InitStruct.Pin = ADC2_CH2_PIN|ADC2_CH3_PIN|ADC2_CH4_PIN|ADC2_CH5_PIN|
-                        ADC2_CH6_PIN|ADC2_CH7_PIN;
+                        ADC2_CH6_PIN;
   GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
   HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
 
@@ -366,7 +352,7 @@ void adc_init(void)
   HAL_ADCEx_Calibration_Start(&hadc2);
 
   HAL_ADC_Start(&hadc2);
-  HAL_ADCEx_MultiModeStart_DMA(&hadc1,adc_data,7);
+  HAL_ADCEx_MultiModeStart_DMA(&hadc1,adc_data,6);
 }
 
 void adc_start(void)
@@ -426,8 +412,6 @@ uint16_t adc_get_channel(adc_ch_t channel)
                    break;
     case ADC_CH12: value=ram_data[DARWIN_ADC_CH12_L]+ram_data[DARWIN_ADC_CH12_H]*256;
                    break;
-    case ADC_CH14: value=ram_data[DARWIN_ADC_CH14_L]+ram_data[DARWIN_ADC_CH14_H]*256;
-                   break;
     default: value=0x0000;
   }
 
@@ -462,8 +446,6 @@ uint16_t adc_get_channel_raw(adc_ch_t channel)
                    break;
     case ADC_CH12: value=(adc_data[5]&0xFFFF0000)>>16;
                    break;
-    case ADC_CH14: value=(adc_data[6]&0xFFFF0000)>>16;
-                   break;
     default: value=0x0000;
   }
 
diff --git a/src/darwin_dyn_master_v2.c b/src/darwin_dyn_master_v2.c
index 5dc5e5d..5c61157 100755
--- a/src/darwin_dyn_master_v2.c
+++ b/src/darwin_dyn_master_v2.c
@@ -11,6 +11,10 @@
 #define TX_EN_PIN                  GPIO_PIN_1               
 #define TX_EN_GPIO_PORT            GPIOA               
 
+#define POWER_GPIO_CLK             __GPIOC_CLK_ENABLE()
+#define POWER_PIN                  GPIO_PIN_3               
+#define POWER_GPIO_PORT            GPIOC                       
+
 /* private variables */
 TDynamixelMaster darwin_dyn_master_v2;
 TTime darwin_dyn_master_v2_timer;
@@ -81,3 +85,15 @@ void darwin_dyn_master_v2_init(void)
   dyn_master_set_return_level(&darwin_dyn_master_v2,return_all);
 }
 
+inline void darwin_dyn_master_v2_enable_power(void)
+{
+  HAL_GPIO_WritePin(POWER_GPIO_PORT,POWER_PIN,GPIO_PIN_SET);
+  ram_data[DARWIN_MM_CNTRL]|=MANAGER_EN_PWR_V2;
+}
+
+inline void darwin_dyn_master_v2_disable_power(void)
+{
+  HAL_GPIO_WritePin(POWER_GPIO_PORT,POWER_PIN,GPIO_PIN_RESET);
+  ram_data[DARWIN_MM_CNTRL]&=(~MANAGER_EN_PWR_V2);
+}
+
diff --git a/src/motion_manager.c b/src/motion_manager.c
index 65f06bf..4d61378 100755
--- a/src/motion_manager.c
+++ b/src/motion_manager.c
@@ -692,6 +692,10 @@ void manager_process_write_cmd(unsigned short int address,unsigned short int len
       darwin_dyn_master_enable_power();
     else
       darwin_dyn_master_disable_power();
+    if(data[DARWIN_MM_CNTRL-address]&MANAGER_EN_PWR_V2)
+      darwin_dyn_master_v2_enable_power();
+    else
+      darwin_dyn_master_v2_disable_power();
   }
   // balance gains
   for(i=MM_BAL_KNEE_GAIN_OFFSET;i<=MM_BAL_HIP_ROLL_GAIN_OFFSET+1;i++)
-- 
GitLab


From 9b9c4030feb28d594b3a6ee585bd778f4a755531 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Fri, 25 Aug 2017 14:56:02 +0200
Subject: [PATCH 08/21] Initial implementation of the stairs module.

---
 include/darwin_registers.h |  48 ++++++
 include/eeprom_init.h      |  22 ++-
 include/stairs.h           |  30 ++++
 src/eeprom.c               |  29 +++-
 src/stairs.c               | 339 +++++++++++++++++++++++++++++++++++++
 5 files changed, 464 insertions(+), 4 deletions(-)
 create mode 100755 include/stairs.h
 create mode 100755 src/stairs.c

diff --git a/include/darwin_registers.h b/include/darwin_registers.h
index 6cf97e2..4a4684d 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 cb4543c..570db02 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 0000000..6907e9f
--- /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 fbb5a1a..b6eae9e 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 0000000..dfa470a
--- /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];
+}
+
-- 
GitLab


From ba9140904e686f00654bf07cd2d3567a9f8f51e1 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Sergi=20Hern=C3=A0ndez=20Juan?= <shernand@iri.upc.edu>
Date: Sun, 27 Aug 2017 21:54:58 +0200
Subject: [PATCH 09/21] First working version of the climb stairs algorithm.
 Integrated the stairs algorithm to the motion manager structure. Tuned the
 stairs algorithm parameters to work in simulation.

---
 Makefile                   |   1 +
 include/darwin_registers.h |  39 +--
 include/eeprom.h           |   2 +-
 include/eeprom_init.h      |  29 ++-
 src/darwin_dyn_slave.c     |   3 +
 src/eeprom.c               |   4 +-
 src/motion_manager.c       |   5 +-
 src/ram.c                  | 147 +----------
 src/stairs.c               | 503 +++++++++++++++++++------------------
 9 files changed, 312 insertions(+), 421 deletions(-)

diff --git a/Makefile b/Makefile
index c31048f..6e95e7d 100755
--- a/Makefile
+++ b/Makefile
@@ -29,6 +29,7 @@ TARGET_FILES+=src/joint_motion.c
 TARGET_FILES+=src/head_tracking.c
 TARGET_FILES+=src/grippers.c
 TARGET_FILES+=src/smart_charger.c
+TARGET_FILES+=src/stairs.c
 
 TARGET_PROCESSOR=STM32F103RE
 
diff --git a/include/darwin_registers.h b/include/darwin_registers.h
index 4a4684d..f12214f 100644
--- a/include/darwin_registers.h
+++ b/include/darwin_registers.h
@@ -95,15 +95,16 @@ 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_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 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_Z_OVERSHOOT              ((unsigned short int)0x0080)
+#define STAIRS_Z_HEIGHT                 ((unsigned short int)0x0081)
+#define STAIRS_HIP_PITCH_OFF            ((unsigned short int)0x0082)
 
 #define LAST_EEPROM_OFFSET              ((unsigned short int)0x00FF)
 
@@ -228,6 +229,8 @@ 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,
@@ -236,7 +239,8 @@ typedef enum {
   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,
+  DARWIN_STAIRS_HIP_PITCH_OFF_L        = STAIRS_HIP_PITCH_OFF,
+  DARWIN_STAIRS_HIP_PITCH_OFF_H        = STAIRS_HIP_PITCH_OFF+1,
 
 //RAM  
   DARWIN_TX_LED_CNTRL              = 0x0100, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2  | bit 1 |      bit 0
@@ -626,10 +630,10 @@ typedef enum {
   DARWIN_SMART_CHARGER_BATT_STATUS_H     = 0x024F,
   DARWIN_SMART_CHARGER_CNTRL             = 0x0250, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 |    bit 1   |   bit 0
                                                    //                                               |  detected  |  enable    
-  DARWIN_GRIPPER_CNTRL                   = 0x0251  //    bit 7    |     bit 6    |    bit 5    |     bit 4    |    bit 3   |    bit 2   |    bit 1    |   bit 0
+  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
 }darwin_registers;
 
 #define      GPIO_BASE_ADDRESS       0x0100
@@ -734,7 +738,14 @@ typedef enum {
 #define      SMART_CHARGER_DET             0x01
 #define      SMART_CHARGER_EN              0x02
 
-
+#define      STAIRS_BASE_ADDRESS     0x0252
+#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_STATUS           0x08
+#define      STAIRS_PHASE            0xF0
 
 #ifdef __cplusplus
 }
diff --git a/include/eeprom.h b/include/eeprom.h
index c200358..c9c2f12 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)0x60)
+#define NB_OF_VAR             ((uint8_t)0x83)
 
 /* Exported types ------------------------------------------------------------*/
 /* Exported macro ------------------------------------------------------------*/
diff --git a/include/eeprom_init.h b/include/eeprom_init.h
index 570db02..db1405a 100644
--- a/include/eeprom_init.h
+++ b/include/eeprom_init.h
@@ -84,25 +84,28 @@ 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           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_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_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_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_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    EEPROM_SIZE                          131
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/src/darwin_dyn_slave.c b/src/darwin_dyn_slave.c
index dd7f12f..b287d06 100755
--- a/src/darwin_dyn_slave.c
+++ b/src/darwin_dyn_slave.c
@@ -13,6 +13,7 @@
 #include "head_tracking.h"
 #include "grippers.h"
 #include "smart_charger.h"
+#include "stairs.h"
 
 /* timer for the execution of the dynamixel slave loop */
 #define     DYN_SLAVE_TIMER                   TIM7
@@ -96,6 +97,8 @@ unsigned char darwin_on_write(unsigned short int address,unsigned short int leng
   // smart charger commands
   if(smart_charger_in_range(address,length))
     smart_charger_process_write_cmd(address,length,data);
+  if(stairs_in_range(address,length))
+    stairs_process_write_cmd(address,length,data);
   // write eeprom
   for(i=address,j=0;i<LAST_EEPROM_OFFSET && i<(address+length);i++,j++)
     EE_WriteVariable(i,data[j]);
diff --git a/src/eeprom.c b/src/eeprom.c
index b6eae9e..142ad7a 100755
--- a/src/eeprom.c
+++ b/src/eeprom.c
@@ -154,7 +154,7 @@ uint16_t eeprom_data[] __attribute__ ((section (".eeprom")))={VALID_PAGE,
                                                               DEFAULT_GRIPPER_RIGHT_MIN_ANGLE&0xFF,      GRIPPER_RIGHT_MIN_ANGLE,
                                                               DEFAULT_GRIPPER_RIGHT_MIN_ANGLE>>8,        GRIPPER_RIGHT_MIN_ANGLE+1,
                                                               DEFAULT_GRIPPER_RIGHT_MAX_FORCE&0xFF,      GRIPPER_RIGHT_MAX_FORCE,
-                                                              DEFAULT_GRIPPER_RIGHT_MAX_FORCE>>8,        GRIPPER_RIGHT_MAX_FORCE+1,        
+                                                              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_STAIRS_PHASE1_TIME&0xFF,           STAIRS_PHASE1_TIME,
@@ -175,6 +175,8 @@ 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,
diff --git a/src/motion_manager.c b/src/motion_manager.c
index 709777c..2e72f34 100755
--- a/src/motion_manager.c
+++ b/src/motion_manager.c
@@ -11,7 +11,7 @@
 #include "imu.h"
 #include "smart_charger.h"
 #include "dyn_battery.h"
-#include "gpio.h"
+#include "stairs.h"
 #include <stdlib.h>
 
 #define MANAGER_TIMER                   TIM5
@@ -198,6 +198,8 @@ void MANAGER_TIMER_IRQHandler(void)
       walking_process();
       // call the gripper process
       grippers_process();
+      // call the stairs process
+      stairs_process();
       // balance the robot
       manager_balance(); 
       // access to smart charger
@@ -469,6 +471,7 @@ void manager_init(uint16_t period_us)
   head_tracking_init(period_us);
   grippers_init(period_us);
   smart_charger_init(period_us);
+  stairs_init(period_us);
 }
 
 uint16_t manager_get_period(void)
diff --git a/src/ram.c b/src/ram.c
index e708a46..be8aa9d 100755
--- a/src/ram.c
+++ b/src/ram.c
@@ -10,152 +10,11 @@ void ram_init(void)
   for(i=0;i<RAM_SIZE;i++)
     ram_data[i]=0x00;
   // read contents from EEPROM to RAM
-  if(EE_ReadVariable(DEVICE_MODEL_OFFSET,&eeprom_data)==0)
-    ram_data[DEVICE_MODEL_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(DEVICE_MODEL_OFFSET+1,&eeprom_data)==0)
-    ram_data[DEVICE_MODEL_OFFSET+1]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(FIRMWARE_VERSION_OFFSET,&eeprom_data)==0)
-    ram_data[FIRMWARE_VERSION_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(DEVICE_ID_OFFSET,&eeprom_data)==0)
-    ram_data[DEVICE_ID_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(BAUDRATE_OFFSET,&eeprom_data)==0)
-    ram_data[BAUDRATE_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(RETURN_DELAY_OFFSET,&eeprom_data)==0)
-    ram_data[RETURN_DELAY_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(MM_PERIOD_OFFSET,&eeprom_data)==0)
-    ram_data[MM_PERIOD_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(MM_PERIOD_OFFSET+1,&eeprom_data)==0)
-    ram_data[MM_PERIOD_OFFSET+1]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(MM_BAL_KNEE_GAIN_OFFSET,&eeprom_data)==0)
-    ram_data[MM_BAL_KNEE_GAIN_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(MM_BAL_KNEE_GAIN_OFFSET+1,&eeprom_data)==0)
-    ram_data[MM_BAL_KNEE_GAIN_OFFSET+1]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(MM_BAL_ANKLE_ROLL_GAIN_OFFSET,&eeprom_data)==0)
-    ram_data[MM_BAL_ANKLE_ROLL_GAIN_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(MM_BAL_ANKLE_ROLL_GAIN_OFFSET+1,&eeprom_data)==0)
-    ram_data[MM_BAL_ANKLE_ROLL_GAIN_OFFSET+1]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(MM_BAL_ANKLE_PITCH_GAIN_OFFSET,&eeprom_data)==0)
-    ram_data[MM_BAL_ANKLE_PITCH_GAIN_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(MM_BAL_ANKLE_PITCH_GAIN_OFFSET+1,&eeprom_data)==0)
-    ram_data[MM_BAL_ANKLE_PITCH_GAIN_OFFSET+1]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(MM_BAL_HIP_ROLL_GAIN_OFFSET,&eeprom_data)==0)
-    ram_data[MM_BAL_HIP_ROLL_GAIN_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(MM_BAL_HIP_ROLL_GAIN_OFFSET+1,&eeprom_data)==0)
-    ram_data[MM_BAL_HIP_ROLL_GAIN_OFFSET+1]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(RETURN_LEVEL_OFFSET,&eeprom_data)==0)
-    ram_data[RETURN_LEVEL_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
-  for(i=0;i<32;i++)
+  for(i=0;i<NB_OF_VAR;i++)
   {
-    if(EE_ReadVariable(MM_SERVO0_OFFSET+i,&eeprom_data)==0)
-      ram_data[DARWIN_MM_SERVO0_OFFSET+i]=(uint8_t)(eeprom_data&0x00FF);
+    if(EE_ReadVariable(i,&eeprom_data)==0)
+      ram_data[i]=(uint8_t)(eeprom_data&0x00FF);
   }
-  if(EE_ReadVariable(WALK_X_OFFSET,&eeprom_data)==0)
-    ram_data[WALK_X_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(WALK_Y_OFFSET,&eeprom_data)==0)
-    ram_data[WALK_Y_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(WALK_Z_OFFSET,&eeprom_data)==0)
-    ram_data[WALK_Z_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(WALK_ROLL_OFFSET,&eeprom_data)==0)
-    ram_data[WALK_ROLL_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(WALK_PITCH_OFFSET,&eeprom_data)==0)
-    ram_data[WALK_PITCH_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(WALK_YAW_OFFSET,&eeprom_data)==0)
-    ram_data[WALK_YAW_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(WALK_HIP_PITCH_OFF,&eeprom_data)==0)
-    ram_data[WALK_HIP_PITCH_OFF]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(WALK_HIP_PITCH_OFF+1,&eeprom_data)==0)
-    ram_data[WALK_HIP_PITCH_OFF+1]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(WALK_PERIOD_TIME,&eeprom_data)==0)
-    ram_data[WALK_PERIOD_TIME]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(WALK_PERIOD_TIME+1,&eeprom_data)==0)
-    ram_data[WALK_PERIOD_TIME+1]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(WALK_DSP_RATIO,&eeprom_data)==0)
-    ram_data[WALK_DSP_RATIO]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(WALK_STEP_FW_BW_RATIO,&eeprom_data)==0)
-    ram_data[WALK_STEP_FW_BW_RATIO]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(WALK_FOOT_HEIGHT,&eeprom_data)==0)
-    ram_data[WALK_FOOT_HEIGHT]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(WALK_SWING_RIGHT_LEFT,&eeprom_data)==0)
-    ram_data[WALK_SWING_RIGHT_LEFT]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(WALK_SWING_TOP_DOWN,&eeprom_data)==0)
-    ram_data[WALK_SWING_TOP_DOWN]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(WALK_PELVIS_OFFSET,&eeprom_data)==0)
-    ram_data[WALK_PELVIS_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(WALK_ARM_SWING_GAIN,&eeprom_data)==0)
-    ram_data[WALK_ARM_SWING_GAIN]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(WALK_MAX_VEL,&eeprom_data)==0)
-    ram_data[WALK_MAX_VEL]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(WALK_MAX_ROT_VEL,&eeprom_data)==0)
-    ram_data[WALK_MAX_ROT_VEL]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(HEAD_PAN_P,&eeprom_data)==0)
-    ram_data[HEAD_PAN_P]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(HEAD_PAN_P+1,&eeprom_data)==0)
-    ram_data[HEAD_PAN_P+1]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(HEAD_PAN_I,&eeprom_data)==0)
-    ram_data[HEAD_PAN_I]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(HEAD_PAN_I+1,&eeprom_data)==0)
-    ram_data[HEAD_PAN_I+1]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(HEAD_PAN_D,&eeprom_data)==0)
-    ram_data[HEAD_PAN_D]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(HEAD_PAN_D+1,&eeprom_data)==0)
-    ram_data[HEAD_PAN_D+1]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(HEAD_PAN_I_CLAMP,&eeprom_data)==0)
-    ram_data[HEAD_PAN_I_CLAMP]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(HEAD_PAN_I_CLAMP+1,&eeprom_data)==0)
-    ram_data[HEAD_PAN_I_CLAMP+1]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(HEAD_TILT_P,&eeprom_data)==0)
-    ram_data[HEAD_TILT_P]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(HEAD_TILT_P+1,&eeprom_data)==0)
-    ram_data[HEAD_TILT_P+1]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(HEAD_TILT_I,&eeprom_data)==0)
-    ram_data[HEAD_TILT_I]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(HEAD_TILT_I+1,&eeprom_data)==0)
-    ram_data[HEAD_TILT_I+1]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(HEAD_TILT_D,&eeprom_data)==0)
-    ram_data[HEAD_TILT_D]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(HEAD_TILT_D+1,&eeprom_data)==0)
-    ram_data[HEAD_TILT_D+1]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(HEAD_TILT_I_CLAMP,&eeprom_data)==0)
-    ram_data[HEAD_TILT_I_CLAMP]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(HEAD_TILT_I_CLAMP+1,&eeprom_data)==0)
-    ram_data[HEAD_TILT_I_CLAMP+1]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(GRIPPER_LEFT_TOP_ID,&eeprom_data)==0)
-    ram_data[GRIPPER_LEFT_TOP_ID]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(GRIPPER_LEFT_BOT_ID,&eeprom_data)==0)
-    ram_data[GRIPPER_LEFT_BOT_ID]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(GRIPPER_LEFT_MAX_ANGLE,&eeprom_data)==0)
-    ram_data[GRIPPER_LEFT_MAX_ANGLE]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(GRIPPER_LEFT_MAX_ANGLE+1,&eeprom_data)==0)
-    ram_data[GRIPPER_LEFT_MAX_ANGLE+1]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(GRIPPER_LEFT_MIN_ANGLE,&eeprom_data)==0)
-    ram_data[GRIPPER_LEFT_MIN_ANGLE]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(GRIPPER_LEFT_MIN_ANGLE+1,&eeprom_data)==0)
-    ram_data[GRIPPER_LEFT_MIN_ANGLE+1]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(GRIPPER_LEFT_MAX_FORCE,&eeprom_data)==0)
-    ram_data[GRIPPER_LEFT_MAX_FORCE]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(GRIPPER_LEFT_MAX_FORCE+1,&eeprom_data)==0)
-    ram_data[GRIPPER_LEFT_MAX_FORCE+1]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(GRIPPER_RIGHT_TOP_ID,&eeprom_data)==0)
-    ram_data[GRIPPER_RIGHT_TOP_ID]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(GRIPPER_RIGHT_BOT_ID,&eeprom_data)==0)
-    ram_data[GRIPPER_RIGHT_BOT_ID]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(GRIPPER_RIGHT_MAX_ANGLE,&eeprom_data)==0)
-    ram_data[GRIPPER_RIGHT_MAX_ANGLE]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(GRIPPER_RIGHT_MAX_ANGLE+1,&eeprom_data)==0)
-    ram_data[GRIPPER_RIGHT_MAX_ANGLE+1]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(GRIPPER_RIGHT_MIN_ANGLE,&eeprom_data)==0)
-    ram_data[GRIPPER_RIGHT_MIN_ANGLE]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(GRIPPER_RIGHT_MIN_ANGLE+1,&eeprom_data)==0)
-    ram_data[GRIPPER_RIGHT_MIN_ANGLE+1]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(GRIPPER_RIGHT_MAX_FORCE,&eeprom_data)==0)
-    ram_data[GRIPPER_RIGHT_MAX_FORCE]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(GRIPPER_RIGHT_MAX_FORCE+1,&eeprom_data)==0)
-    ram_data[GRIPPER_RIGHT_MAX_FORCE+1]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(SMART_CHARGER_PERIOD,&eeprom_data)==0)
-    ram_data[SMART_CHARGER_PERIOD]=(uint8_t)(eeprom_data&0x00FF);
-  if(EE_ReadVariable(SMART_CHARGER_PERIOD+1,&eeprom_data)==0)
-    ram_data[SMART_CHARGER_PERIOD+1]=(uint8_t)(eeprom_data&0x00FF);
-
 }
 
 inline void ram_read_byte(uint16_t address,uint8_t *data)
diff --git a/src/stairs.c b/src/stairs.c
index dfa470a..5cefc8e 100755
--- a/src/stairs.c
+++ b/src/stairs.c
@@ -2,39 +2,40 @@
 #include "darwin_kinematics.h"
 #include "darwin_math.h"
 #include "ram.h"
+#include <stdio.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;
+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;
 
 // 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;
+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;
 
 // internal offset variables
-float m_Hip_Pitch_Offset;
-float m_X_Offset;
-float m_Y_Offset;
-float m_Z_Offset;
+float stairs_Hip_Pitch_Offset;
+float stairs_X_Offset;
+float stairs_Y_Offset;
+float stairs_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;
+float stairs_Time;
+float stairs_period;
+float stairs_shift_weight_left_time;
+float stairs_rise_right_foot_time;
+float stairs_advance_right_foot_time;
+float stairs_contact_right_foot_time;
+float stairs_shift_weight_right_time;
+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 m_Ctrl_Running;
+uint8_t stairs_Ctrl_Running;
 
 // private functions
 
@@ -45,11 +46,9 @@ void stairs_init(uint16_t period_us)
 
 
   // initialize internal control variables
-  m_Time=0;
+  stairs_Time=0;
   stairs_period=((float)period_us)/1000.0;
-  m_Ctrl_Running=0x00;
-
-  stairs_process();
+  stairs_Ctrl_Running=0x00;
 }
 
 inline void stairs_set_period(uint16_t period_us)
@@ -64,18 +63,39 @@ inline uint16_t stairs_get_period(void)
 
 void stairs_start(void)
 {
-  ram_data[DARWIN_WALK_CNTRL]|=WALK_STATUS;
-  m_Ctrl_Running=0x01;
+  // load all parameters
+  stairs_shift_weight_left_time=((float)ram_data[DARWIN_STAIRS_PHASE1_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE1_TIME_H]<<8));
+  stairs_rise_right_foot_time=((float)ram_data[DARWIN_STAIRS_PHASE2_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE2_TIME_H]<<8));
+  stairs_advance_right_foot_time=((float)ram_data[DARWIN_STAIRS_PHASE3_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE3_TIME_H]<<8));
+  stairs_contact_right_foot_time=((float)ram_data[DARWIN_STAIRS_PHASE4_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE4_TIME_H]<<8));
+  stairs_shift_weight_right_time=((float)ram_data[DARWIN_STAIRS_PHASE5_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE5_TIME_H]<<8));
+  stairs_rise_left_foot_time=((float)ram_data[DARWIN_STAIRS_PHASE6_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE6_TIME_H]<<8));
+  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_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_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]));
+  // start operation
+  ram_data[DARWIN_STAIRS_CNTRL]|=STAIRS_STATUS;
+  stairs_Ctrl_Running=0x01;
 }
 
 void stairs_stop(void)
 {
-  m_Ctrl_Running=0x00;
+  stairs_Ctrl_Running=0x00;
 }
 
 uint8_t is_climbing_stairs(void)
 {
-  if(ram_data[DARWIN_WALK_CNTRL]&WALK_STATUS)
+  if(ram_data[DARWIN_STAIRS_CNTRL]&STAIRS_STATUS)
     return 0x01;
   else 
     return 0x00;
@@ -84,225 +104,221 @@ uint8_t is_climbing_stairs(void)
 // 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];
+  float angle[14]={0},ep[12]={0};
 
-  if(m_Time==0)
+  if(ram_data[DARWIN_STAIRS_CNTRL]&STAIRS_STATUS)
   {
-    update_param_time();
-    ram_data[DARWIN_WALK_CNTRL]&=(~WALK_PHASE);
-    ram_data[DARWIN_WALK_CNTRL]|=PHASE0;
-    if(m_Ctrl_Running==0x00)
+    ram_data[DARWIN_STAIRS_CNTRL]&=(~STAIRS_PHASE);
+    if(stairs_Time>=0 && stairs_Time<=stairs_shift_weight_left_time)
     {
-      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;
-      }
+      //1
+      ep[0]=stairs_X_Offset;
+      ep[1]=stairs_Y_Offset/2.0+(stairs_Y_shift_amplitude/stairs_shift_weight_left_time)*stairs_Time;
+      ep[2]=stairs_Z_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[8]=stairs_Z_Offset;
+      ram_data[DARWIN_STAIRS_CNTRL]|=SHIFT_WEIGHT_LEFT;
     }
-  }
-  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)
+    else if(stairs_Time>stairs_shift_weight_left_time && stairs_Time<=stairs_rise_right_foot_time)
     {
-      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;
-      }
+      //2
+      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[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);
+      ram_data[DARWIN_STAIRS_CNTRL]|=RISE_RIGHT_FOOT;
     }
-  }
-  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;*/
+    else if(stairs_Time>stairs_rise_right_foot_time && stairs_Time<=stairs_advance_right_foot_time)
+    {   
+      //3
+      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[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[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);
+      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
+      ep[0]=stairs_X_Offset;
+      ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-10.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;
+      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);
+      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[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;
+      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;
+      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;
+      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);
+      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[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);
+      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
+      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[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;
+      ram_data[DARWIN_STAIRS_CNTRL]|=CONTACT_LEFT_FOOT;
+    }
+    else if(stairs_Time>stairs_contact_left_foot_time && stairs_Time<=stairs_center_weight_time)
+    {
+      //9
+      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[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;
+      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);
+      stairs_Ctrl_Running=0x00;
+    }
+    printf("%x,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n",ram_data[DARWIN_STAIRS_CNTRL]&STAIRS_PHASE,ep[0],ep[1],ep[2],ep[3],ep[4],ep[5],ep[6],ep[7],ep[8],ep[9],ep[10],ep[11]);
+    
+    if(ram_data[DARWIN_STAIRS_CNTRL]&STAIRS_STATUS)
+      stairs_Time += stairs_period;
+    else
+      stairs_Time=0;
 
-  // 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);
-  }
+    // 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))
+    {
+      printf("Impossible to find inverse kinematics solution\n");
+      return;// Do not use angles
+    }
 
-  if(ram_data[DARWIN_WALK_CNTRL]&WALK_STATUS)
-  {
-    m_Time += stairs_period;
-    if(m_Time >= m_PeriodTime)
-      m_Time = 0;
+    // 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])/PI)*65536.0;
+    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;
+    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])/PI)*65536.0;
+    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;
+    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;
   }
-
-  // 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))
+  if(ram_in_window(STAIRS_BASE_ADDRESS,STAIRS_MEM_LENGTH,address,length) ||
+     ram_in_window(STAIRS_EEPROM_ADDRESS,STAIRS_EEPROM_LENGTH,address,length))
     return 0x01;
   else
     return 0x00;
@@ -318,21 +334,14 @@ void stairs_process_write_cmd(unsigned short int address,unsigned short int leng
   uint16_t i;
 
   // walk control
-  if(ram_in_range(DARWIN_WALK_CNTRL,address,length))
+  if(ram_in_range(DARWIN_STAIRS_CNTRL,address,length))
   {
-    if(data[DARWIN_WALK_CNTRL-address]&WALK_START)
+    if(data[DARWIN_STAIRS_CNTRL-address]&STAIRS_START)
       stairs_start();
-    if(data[DARWIN_WALK_CNTRL-address]&WALK_STOP)
+    if(data[DARWIN_STAIRS_CNTRL-address]&STAIRS_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++)
+  for(i=STAIRS_EEPROM_ADDRESS;i<=STAIRS_EEPROM_ADDRESS+STAIRS_EEPROM_LENGTH;i++)
     if(ram_in_range(i,address,length))
       ram_data[i]=data[i-address];
 }
-- 
GitLab


From 7d7b711d334084db6c9052e69700757c19b50ef3 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Sergi=20Hern=C3=A0ndez=20Juan?= <shernand@iri.upc.edu>
Date: Sun, 27 Aug 2017 23:21:07 +0200
Subject: [PATCH 10/21] Changed all the hardcoded values to registers in the
 dynamixel interface. Cleaned up the stairs code.

---
 include/darwin_registers.h |  33 ++++--
 include/eeprom.h           |   2 +-
 include/eeprom_init.h      |  32 +++---
 src/eeprom.c               |  16 ++-
 src/stairs.c               | 206 ++++++++++++++++++++-----------------
 5 files changed, 167 insertions(+), 122 deletions(-)

diff --git a/include/darwin_registers.h b/include/darwin_registers.h
index f12214f..04c6658 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 c9c2f12..5d22788 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 db1405a..adf5729 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 142ad7a..ad139df 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 5cefc8e..dd9668f 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);
-- 
GitLab


From 787cde6e330edbd489ff878bd9acc69ffd6a2851 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Sergi=20Hern=C3=A0ndez=20Juan?= <shernand@iri.upc.edu>
Date: Sun, 27 Aug 2017 23:22:31 +0200
Subject: [PATCH 11/21] Removed the printf's functins used for debugging.

---
 src/stairs.c | 5 -----
 1 file changed, 5 deletions(-)

diff --git a/src/stairs.c b/src/stairs.c
index dd9668f..22ac2e4 100755
--- a/src/stairs.c
+++ b/src/stairs.c
@@ -2,7 +2,6 @@
 #include "darwin_kinematics.h"
 #include "darwin_math.h"
 #include "ram.h"
-#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} stairs_phase_t;
@@ -289,7 +288,6 @@ void stairs_process(void)
       ram_data[DARWIN_STAIRS_CNTRL]&=(~STAIRS_STATUS);
       stairs_Ctrl_Running=0x00;
     }
-    printf("%x,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n",ram_data[DARWIN_STAIRS_CNTRL]&STAIRS_PHASE,ep[0],ep[1],ep[2],ep[3],ep[4],ep[5],ep[6],ep[7],ep[8],ep[9],ep[10],ep[11]);
     
     if(ram_data[DARWIN_STAIRS_CNTRL]&STAIRS_STATUS)
       stairs_Time += stairs_period;
@@ -299,10 +297,7 @@ void stairs_process(void)
     // 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))
-    {
-      printf("Impossible to find inverse kinematics solution\n");
       return;// Do not use angles
-    }
 
     // Compute motor value
     if(manager_get_module(R_HIP_YAW)==MM_WALKING)
-- 
GitLab


From 4c6f428355ee47642524c75a6d2302c8c7755aab Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Sergi=20Hern=C3=A0ndez=20Juan?= <shernand@iri.upc.edu>
Date: Mon, 28 Aug 2017 00:31:07 +0200
Subject: [PATCH 12/21] Added a function to get the current phase of the stairs
 algorithm.

---
 include/stairs.h | 1 +
 src/stairs.c     | 5 +++++
 2 files changed, 6 insertions(+)

diff --git a/include/stairs.h b/include/stairs.h
index 6907e9f..08f4ba5 100755
--- a/include/stairs.h
+++ b/include/stairs.h
@@ -15,6 +15,7 @@ inline uint16_t stairs_get_period(void);
 void stairs_start(void);
 void stairs_stop(void);
 uint8_t is_climbing_stairs(void);
+uint8_t stairs_get_phase(void);
 
 // operation functions
 uint8_t stairs_in_range(unsigned short int address, unsigned short int length);
diff --git a/src/stairs.c b/src/stairs.c
index 22ac2e4..cf09087 100755
--- a/src/stairs.c
+++ b/src/stairs.c
@@ -112,6 +112,11 @@ uint8_t is_climbing_stairs(void)
     return 0x00;
 }
 
+uint8_t stairs_get_phase(void)
+{
+  return (int8_t)ram_data[DARWIN_STAIRS_CNTRL]&STAIRS_PHASE;
+}
+
 // motion manager interface functions
 void stairs_process(void)
 {
-- 
GitLab


From 20897b8df7d7335ed685825863b31d25cf60afd9 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Sergi=20Hern=C3=A0ndez=20Juan?= <shernand@iri.upc.edu>
Date: Mon, 28 Aug 2017 23:12:06 +0200
Subject: [PATCH 13/21] Modified the stairs trajectory to avoid collisions with
 the stairs.

---
 src/stairs.c | 14 +++++++-------
 1 file changed, 7 insertions(+), 7 deletions(-)

diff --git a/src/stairs.c b/src/stairs.c
index cf09087..6a2eaf3 100755
--- a/src/stairs.c
+++ b/src/stairs.c
@@ -228,7 +228,7 @@ void stairs_process(void)
       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[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;
@@ -242,13 +242,13 @@ void stairs_process(void)
       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+stairs_P_shift_amplitude*delta;
+      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;
+      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+stairs_P_shift_amplitude*delta;
+      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;
     }
@@ -260,13 +260,13 @@ void stairs_process(void)
       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;
+      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;
+      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;
+      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;
     }
-- 
GitLab


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 14/21] 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


From 1d5bb0ca753662c072ff868c308dec2aca6058d2 Mon Sep 17 00:00:00 2001
From: ferranmafe <darwin@darwin.users.iri.prv>
Date: Thu, 31 Aug 2017 18:54:48 +0200
Subject: [PATCH 15/21] Added the enable and disable power functions for the
 second dynamixel bus.

---
 src/cm730_fw.c             | 1 +
 src/darwin_dyn_master_v2.c | 1 +
 src/motion_manager.c       | 4 +++-
 3 files changed, 5 insertions(+), 1 deletion(-)

diff --git a/src/cm730_fw.c b/src/cm730_fw.c
index 5c1040f..7693bac 100755
--- a/src/cm730_fw.c
+++ b/src/cm730_fw.c
@@ -7,6 +7,7 @@
 #include "darwin_time.h"
 #include "darwin_dyn_slave.h"
 #include "darwin_dyn_master.h"
+#include "darwin_dyn_master_v2.h"
 #include "motion_manager.h"
 #include "action.h"
 #include "action_id.h"
diff --git a/src/darwin_dyn_master_v2.c b/src/darwin_dyn_master_v2.c
index 5c61157..6152125 100755
--- a/src/darwin_dyn_master_v2.c
+++ b/src/darwin_dyn_master_v2.c
@@ -83,6 +83,7 @@ void darwin_dyn_master_v2_init(void)
   /* configure dynamixel master module */
   dyn_master_set_rx_timeout(&darwin_dyn_master_v2,50);
   dyn_master_set_return_level(&darwin_dyn_master_v2,return_all);
+  darwin_dyn_master_v2_disable_power();
 }
 
 inline void darwin_dyn_master_v2_enable_power(void)
diff --git a/src/motion_manager.c b/src/motion_manager.c
index 2e72f34..805fc18 100755
--- a/src/motion_manager.c
+++ b/src/motion_manager.c
@@ -365,6 +365,7 @@ void manager_init(uint16_t period_us)
     
   darwin_dyn_master_disable_power();
 
+  darwin_dyn_master_v2_enable_power();
   // detect the servos connected on the v2 bus
   dyn_master_scan(&darwin_dyn_master_v2,&num,servo_ids); 
   ram_data[DARWIN_MM_NUM_SERVOS]+=num;
@@ -425,7 +426,8 @@ void manager_init(uint16_t period_us)
       default: break;
     }
   }
-  
+
+  darwin_dyn_master_v2_disable_power();
   
   ram_data[DARWIN_MM_PRESENT_SERVOS1]=(present_servos&0x000000FF);
   ram_data[DARWIN_MM_PRESENT_SERVOS2]=((present_servos&0x0000FF00)>>8);
-- 
GitLab


From 3e658674e8fcf05018d5ffe7f49560c60f2d4b6e Mon Sep 17 00:00:00 2001
From: ferranmafe <darwin@darwin.users.iri.prv>
Date: Thu, 31 Aug 2017 20:55:10 +0200
Subject: [PATCH 16/21] Changed to the remmaped version of the USART1 module.

---
 Makefile | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Makefile b/Makefile
index 6e95e7d..ff8179a 100755
--- a/Makefile
+++ b/Makefile
@@ -95,7 +95,7 @@ TARGET_FILES+=$(HAL_PATH)/src/stm32f1xx_hal.c
 
 TARGET_FILES+=$(USART_PATH)/src/usart3.c
 TARGET_FILES+=$(USART_PATH)/src/usart2.c
-TARGET_FILES+=$(USART_PATH)/src/usart1.c
+TARGET_FILES+=$(USART_PATH)/src/usart1_remap.c
 
 DARWIN_OBJS_TMP = $(notdir $(TARGET_FILES:.c=.o))
 DARWIN_OBJS = $(patsubst %,$(BUILD_PATH)/%,$(DARWIN_OBJS_TMP))
-- 
GitLab


From fb1a3d8849bdf6e621cceaf0ece593333f691ef9 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Sergi=20Hern=C3=A0ndez=20Juan?= <shernand@iri.upc.edu>
Date: Sat, 2 Sep 2017 15:36:14 +0200
Subject: [PATCH 17/21] Modified the stairs up algorithm to make it more stable
 in the real robot.

---
 src/stairs.c | 30 +++++++++++++++---------------
 1 file changed, 15 insertions(+), 15 deletions(-)

diff --git a/src/stairs.c b/src/stairs.c
index 6ad7d3c..3947bd6 100755
--- a/src/stairs.c
+++ b/src/stairs.c
@@ -136,7 +136,7 @@ void stairs_process(void)
         //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[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;
@@ -154,7 +154,7 @@ void stairs_process(void)
         //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[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;
@@ -172,7 +172,7 @@ void stairs_process(void)
         //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[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;
@@ -190,7 +190,7 @@ void stairs_process(void)
         //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[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;
@@ -208,16 +208,16 @@ void stairs_process(void)
         //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[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[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-(2.0*stairs_P_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;
       }
@@ -226,16 +226,16 @@ void stairs_process(void)
         //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[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[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[10]=stairs_P_Offset-stairs_P_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;
       }
@@ -244,16 +244,16 @@ void stairs_process(void)
         //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[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[4]=stairs_P_Offset-2.0*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-stairs_Z_overshoot;
         ep[9]=stairs_R_Offset+stairs_R_shift_amplitude;
-        ep[10]=stairs_P_Offset-stairs_P_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;
         ram_data[DARWIN_STAIRS_CNTRL]|=ADVANCE_LEFT_FOOT;
       }
@@ -262,7 +262,7 @@ void stairs_process(void)
         //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[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;
@@ -280,7 +280,7 @@ void stairs_process(void)
         //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[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;
-- 
GitLab


From 2c9cf1d62bc394b271f036bac8ce2464d06aa127 Mon Sep 17 00:00:00 2001
From: ferranmafe <darwin@darwin.users.iri.prv>
Date: Sat, 2 Sep 2017 15:44:08 +0200
Subject: [PATCH 18/21] Changed the size of the EEPROM memory region for the
 stairs module.

---
 include/darwin_registers.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/include/darwin_registers.h b/include/darwin_registers.h
index cfa36fc..b84afcc 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
-- 
GitLab


From 05bea09c4b3655caaf29ca932c94041a7323eb6f Mon Sep 17 00:00:00 2001
From: shernand <darwin@darwin.users.iri.prv>
Date: Sat, 2 Sep 2017 19:16:54 +0200
Subject: [PATCH 19/21] Added the PID parameters values to the stairs
 algorithm. Changed the coding of the roll and pitch shift parameters.
 Modified the upstairs algorithm to work in the real robot.

---
 include/eeprom_init.h |  4 +--
 src/stairs.c          | 74 +++++++++++++++++++++++++++++++------------
 2 files changed, 55 insertions(+), 23 deletions(-)

diff --git a/include/eeprom_init.h b/include/eeprom_init.h
index adf5729..09eca5c 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 3947bd6..18adb7f 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;//-stairs_Z_overshoot*delta;
+        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;//-stairs_Z_overshoot;
         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;//-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;
@@ -381,13 +381,13 @@ void stairs_process(void)
         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[4]=stairs_P_Offset+stairs_P_shift_amplitude-(4.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[10]=stairs_P_Offset+stairs_P_shift_amplitude-(4.0*stairs_P_shift_amplitude)*delta;
         ep[11]=stairs_A_Offset+stairs_A_shift_amplitude;
         ram_data[DARWIN_STAIRS_CNTRL]|=SHIFT_WEIGHT_RIGHT;
       }
@@ -399,13 +399,13 @@ void stairs_process(void)
         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[4]=stairs_P_Offset-3.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[10]=stairs_P_Offset-3.0*stairs_P_shift_amplitude;
         ep[11]=stairs_A_Offset+stairs_A_shift_amplitude;
         ram_data[DARWIN_STAIRS_CNTRL]|=RISE_LEFT_FOOT;
       }
@@ -417,13 +417,13 @@ void stairs_process(void)
         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[4]=stairs_P_Offset-3.0*stairs_P_shift_amplitude+3.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[10]=stairs_P_Offset-3.0*stairs_P_shift_amplitude+3.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;
       }
@@ -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;
+    }
   }
 }
 
-- 
GitLab


From c8bf191169e6b76f093cf979fb35efde8b88e7c2 Mon Sep 17 00:00:00 2001
From: shernand <darwin@darwin.users.iri.prv>
Date: Sat, 2 Sep 2017 20:53:32 +0200
Subject: [PATCH 20/21] Modified the downstairs algorithm to work in the real
 robot.

---
 src/stairs.c | 34 +++++++++++++++++-----------------
 1 file changed, 17 insertions(+), 17 deletions(-)

diff --git a/src/stairs.c b/src/stairs.c
index 18adb7f..68795d9 100755
--- a/src/stairs.c
+++ b/src/stairs.c
@@ -233,7 +233,7 @@ void stairs_process(void)
         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[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;
@@ -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-(4.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-(4.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-3.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-3.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-3.0*stairs_P_shift_amplitude+3.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-3.0*stairs_P_shift_amplitude+3.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;
-- 
GitLab


From 834104401f6de85348fc4a1c3f9697eae676d2f0 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Tue, 5 Sep 2017 01:11:10 +0200
Subject: [PATCH 21/21] Solved a problem with the EEPROM leveleing algorithm.

---
 src/eeprom.c | 9 +++++----
 1 file changed, 5 insertions(+), 4 deletions(-)

diff --git a/src/eeprom.c b/src/eeprom.c
index ad139df..fd7d1f0 100755
--- a/src/eeprom.c
+++ b/src/eeprom.c
@@ -773,7 +773,7 @@ static uint16_t EE_PageTransfer(uint16_t VirtAddress, uint16_t Data)
   HAL_StatusTypeDef flashstatus = HAL_OK;
   uint32_t newpageaddress = EEPROM_START_ADDRESS;
   uint32_t oldpageid = 0;
-  uint16_t validpage = PAGE0, varidx = 0;
+  uint16_t validpage = PAGE0, varidx = 0, address=0;
   uint16_t eepromstatus = 0, readstatus = 0;
   uint32_t page_error = 0;
   FLASH_EraseInitTypeDef s_eraseinit;
@@ -821,15 +821,16 @@ static uint16_t EE_PageTransfer(uint16_t VirtAddress, uint16_t Data)
   /* Transfer process: transfer variables from old to the new active page */
   for (varidx = 0; varidx < NB_OF_VAR; varidx++)
   {
-    if (eeprom_data[varidx] != VirtAddress)  /* Check each variable except the one passed as parameter */
+    address=((__IO uint16_t*)oldpageid)[1+varidx*2];
+    if (address != VirtAddress)  /* Check each variable except the one passed as parameter */
     {
       /* Read the other last variable updates */
-      readstatus = EE_ReadVariable(eeprom_data[varidx], &DataVar);
+      readstatus = EE_ReadVariable(address, &DataVar);
       /* In case variable corresponding to the virtual address was found */
       if (readstatus != 0x1)
       {
         /* Transfer the variable to the new active page */
-        eepromstatus = EE_VerifyPageFullWriteVariable(eeprom_data[varidx], DataVar);
+        eepromstatus = EE_VerifyPageFullWriteVariable(address, DataVar);
         /* If program operation was failed, a Flash error code is returned */
         if (eepromstatus != HAL_OK)
         {
-- 
GitLab