diff --git a/communications/Makefile b/communications/Makefile
index a2382b4cbf8c7873331f76ee4ba386823dce806e..cd714da6eb59cc665f3559b63d0fa3035d3048ca 100644
--- a/communications/Makefile
+++ b/communications/Makefile
@@ -20,7 +20,7 @@ ARFLAGS=rsc
 
 .PHONY: show_banner all
 
-all: show_banner $(PROJECT).a examples
+all: show_banner $(PROJECT).a 
 
 show_banner:
 	@echo "------------------------------------------------------";
diff --git a/communications/include/comm_cfg.h b/communications/include/comm_cfg.h
new file mode 100644
index 0000000000000000000000000000000000000000..454edd881749c3ab5c5fa892726b6ca083a13006
--- /dev/null
+++ b/communications/include/comm_cfg.h
@@ -0,0 +1,24 @@
+#ifndef _COMM_CFG_H
+#define _COMM_CFG_H
+
+#ifndef DYN_MASTER_MAX_TX_BUFFER_LEN
+  #define DYN_MASTER_MAX_TX_BUFFER_LEN            128
+#endif
+
+#ifndef DYN_MASTER_MAX_RX_BUFFER_LEN
+  #define DYN_MASTER_MAX_RX_BUFFER_LEN            128
+#endif
+
+#ifndef DYN_MASTER_DEFAULT_BAUDRATE
+  #define DYN_MASTER_DEFAULT_BAUDRATE             1000000
+#endif
+
+#ifndef DYN_MASTER_DEFAULT_TIMEOUT_US
+  #define DYN_MASTER_DEFAULT_TIMEOUT_US           10000
+#endif
+
+#ifndef SERIAL_CONSOLE_MAX_BUFFER_LEN
+  #define SERIAL_CONSOLE_MAX_BUFFER_LEN           128
+#endif
+
+#endif
diff --git a/communications/include/dynamixel_master.h b/communications/include/dynamixel_master.h
index 9e1bf8198b121d50845c112b57a5e8075dcab59b..9fa62737f3c26e3e75d2bfbc888cc93d2524dd66 100644
--- a/communications/include/dynamixel_master.h
+++ b/communications/include/dynamixel_master.h
@@ -3,14 +3,6 @@
 
 #include "dynamixel.h"
 
-#ifndef MAX_DYN_MASTER_TX_BUFFER_LEN
-  #define       MAX_DYN_MASTER_TX_BUFFER_LEN      128
-#endif
-
-#ifndef MAX_DYN_MASTER_RX_BUFFER_LEN
-  #define       MAX_DYN_MASTER_RX_BUFFER_LEN      128
-#endif
-
 /* public functions */
 void dyn_master_init(void);
 void dyn_master_set_rx_timeout(uint16_t timeout_ms);
diff --git a/communications/include/serial_console.h b/communications/include/serial_console.h
index eb0dcebd55867d57d3ba7e4f2eb3af48d3821e69..2624cfa550762791faee2eec945068763002ddb4 100644
--- a/communications/include/serial_console.h
+++ b/communications/include/serial_console.h
@@ -4,8 +4,6 @@
 #include <avr/io.h>
 #include <avr/interrupt.h>
 
-#define SERIAL_CONSOLE_BUFFER    128
-
 void serial_console_init(uint32_t baudrate);
 
 #endif
diff --git a/communications/src/dynamixel_master.c b/communications/src/dynamixel_master.c
index f44efa6dcbea8cc8b43c9f695719a990dbf22b9f..c1b445ac1b8fc594fa0d31aba5c2a31e909c09a3 100644
--- a/communications/src/dynamixel_master.c
+++ b/communications/src/dynamixel_master.c
@@ -1,10 +1,11 @@
+#include "comm_cfg.h"
 #include "dynamixel_master.h"
 
 /* private variables */
-uint8_t dyn_master_tx_buffer[MAX_DYN_MASTER_TX_BUFFER_LEN];
+uint8_t dyn_master_tx_buffer[DYN_MASTER_MAX_TX_BUFFER_LEN];
 uint8_t dyn_master_sent_bytes;
 volatile uint8_t dyn_master_sent_done;
-uint8_t dyn_master_rx_buffer[MAX_DYN_MASTER_RX_BUFFER_LEN];
+uint8_t dyn_master_rx_buffer[DYN_MASTER_MAX_RX_BUFFER_LEN];
 uint8_t dyn_master_received_bytes;
 volatile uint8_t dyn_master_packet_ready;
 return_level_t dyn_master_return_level;
@@ -204,10 +205,10 @@ void dyn_master_init(void)
   // bit3: stop bit(0 = stop bit 1, 1 = stop bit 2)
   // bit2,bit1: data size(11 = 8bit)
   UCSR0C = 0b00000110;
-  dyn_master_set_baudrate((uint32_t)1000000);
+  dyn_master_set_baudrate((uint32_t)DYN_MASTER_DEFAULT_BAUDRATE);
 
   /* initialize the timeout timer */
-  dyn_master_rx_timeout_us=10000;
+  dyn_master_rx_timeout_us=DYN_MASTER_DEFAULT_TIMEOUT_US;
   TCCR0A=0x00;// normal mode, output disabled on both channels
   TCCR0B=0x00;// normal mode, prescaler 0 (timer disabled)
   TIMSK0=0x00;// all interrupts disabled
diff --git a/communications/src/serial_console.c b/communications/src/serial_console.c
index 55496d5348b7332987f5199d55d46dea5ae1a972..7842eb7d91bf8866bd014625a7a30f234339bbfd 100644
--- a/communications/src/serial_console.c
+++ b/communications/src/serial_console.c
@@ -1,8 +1,9 @@
+#include "comm_cfg.h"
 #include "serial_console.h"
 #include <stdio.h>
 
 /* private variables */
-volatile uint8_t serial_console_buffer[SERIAL_CONSOLE_BUFFER];
+volatile uint8_t serial_console_buffer[SERIAL_CONSOLE_MAX_BUFFER_LEN];
 volatile uint8_t serial_console_read_ptr;
 volatile uint8_t serial_console_write_ptr;
 volatile uint8_t serial_console_num_data;
@@ -11,10 +12,10 @@ static FILE *device;
 /* interrupt handlers */
 SIGNAL(USART1_RX_vect)
 {
-  if(serial_console_num_data<SERIAL_CONSOLE_BUFFER)
+  if(serial_console_num_data<SERIAL_CONSOLE_MAX_BUFFER_LEN)
   {
     serial_console_buffer[serial_console_write_ptr]=UDR1;
-    serial_console_write_ptr=(serial_console_write_ptr+1)%SERIAL_CONSOLE_BUFFER;
+    serial_console_write_ptr=(serial_console_write_ptr+1)%SERIAL_CONSOLE_MAX_BUFFER_LEN;
     serial_console_num_data++;
   }
 }
@@ -49,7 +50,7 @@ int serial_console_getchar(FILE *dev)
 
   while(serial_console_num_data==0);
   rx=serial_console_buffer[serial_console_read_ptr];
-  serial_console_read_ptr=(serial_console_read_ptr+1)%SERIAL_CONSOLE_BUFFER;
+  serial_console_read_ptr=(serial_console_read_ptr+1)%SERIAL_CONSOLE_MAX_BUFFER_LEN;
   serial_console_num_data--;
   if(rx=='\r')
     rx='\n';
diff --git a/controllers/Makefile b/controllers/Makefile
index 3afb4f27ee39fbe6e3ab24d166bf02aaa26da68e..1c0c8bca77ce8044316e1d248b82b8849706f9f4 100755
--- a/controllers/Makefile
+++ b/controllers/Makefile
@@ -1,19 +1,22 @@
 #AVR-GCC  Makefile
 
 PROJECT=libcontrollers
-SOURCES=src/cm510/cm510.c src/cm510/gpio.c src/cm510/adc.c src/cm510/buzzer.c
+SOURCES=src/cm510.c src/gpio.c src/adc.c src/buzzer.c src/user_time.c
 OBJS=$(SOURCES:.c=.o)
 SRC_DIR=./src/ 
 BIN_DIR=./build/
 LIB_DIR=./lib/
+DEV_DIR=../dyn_devices/
 COMM_DIR=../communications/
+MAN_DIR=../motion/
+CONT_DIR=./
 CC=avr-gcc
 OBJCOPY=avr-ar
 MMCU=atmega2561
 
-INC_DIRS=-I./include/ -I$(COMM_DIR)include
+INC_DIRS=-I$(CONT_DIR)include/ -I$(COMM_DIR)include -I$(DEV_DIR)include -I$(MAN_DIR)include
 
-LIBS=$(COMM_DIR)lib/libcomm.a
+LIBS=$(COMM_DIR)lib/libcomm.a $(DEV_DIR)lib/libdyn_devices.a $(MAN_DIR)lib/libmotion_manager.a
 
 CFLAGS=-mmcu=$(MMCU) -Wall -O3 -DF_CPU=16000000UL -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
 
@@ -21,7 +24,7 @@ ARFLAGS=rsc
 
 .PHONY: all show_banner clean
 
-all: show_banner communications dyn_devices $(PROJECT).a examples
+all: show_banner communications dyn_devices motion_manager $(PROJECT).a
 
 show_banner:
 	@echo "------------------------------------------------------";
@@ -47,8 +50,14 @@ communications:
 dyn_devices:
 	$(MAKE) -C ../dyn_devices
 
+motion_manager:
+	$(MAKE) -C ../motion
+
 examples: 
 	$(MAKE) -C src/examples
+	$(MAKE) -C ../communications examples
+	$(MAKE) -C ../dyn_devices examples
+	$(MAKE) -C ../motion examples
 
 download: 
 	$(MAKE) -C src/examples download
@@ -59,3 +68,4 @@ clean:
 	$(MAKE) -C src/examples clean
 	$(MAKE) -C ../communications clean
 	$(MAKE) -C ../dyn_devices clean
+	$(MAKE) -C ../motion clean
diff --git a/controllers/include/adc.h b/controllers/include/adc.h
index 6235d1d444005372af713495d616ba701e31bcf3..a5b45094cf4da6a4d3b5f3280aeef76c3c175a63 100644
--- a/controllers/include/adc.h
+++ b/controllers/include/adc.h
@@ -7,10 +7,6 @@
 // ADC channel identifiers
 typedef enum {ADC_VCC=0,ADC_PORT_1=1,ADC_PORT_2=2,ADC_PORT_3=3,ADC_PORT_4=4,ADC_PORT_5=5,ADC_PORT_6=6} adc_t;
 #define         NUM_ADC               7
-// number of samples to average
-#define         ADC_MAX_NUM_SAMPLES   16
-#define         ADC_SAMPLE_PERIOD_MS  10
-#define         ADC_VOLTAGE_ALARM_TH  559
 
 /**
  * \brief Function to initialize the ADC module
diff --git a/controllers/include/cm510.h b/controllers/include/cm510.h
index dd87e115ab575d2c88763bf97ba10b898b270dd1..4eba4049da0918f60c711a7d96bf024224668b1c 100755
--- a/controllers/include/cm510.h
+++ b/controllers/include/cm510.h
@@ -18,6 +18,15 @@
 #ifndef _CM510_H
 #define _CM510_H
 
+#include "cont_cfg.h"
+#include "motion_cfg.h"
+#include "comm_cfg.h"
+#include "gpio.h"
+#include "adc.h"
+#include "buzzer.h"
+#include "motion_manager.h"
+#include "user_time.h"
+
 // general interface
 /**
  * \brief Function to initialize all the cm510 modules
diff --git a/controllers/include/cm510_cfg.h b/controllers/include/cm510_cfg.h
new file mode 100644
index 0000000000000000000000000000000000000000..e46833f4107bd561f7ab31449eb09f2758ac060b
--- /dev/null
+++ b/controllers/include/cm510_cfg.h
@@ -0,0 +1,41 @@
+#ifndef _CM510_CFG_H
+#define _CM510_CFG_H
+
+#include "buzzer.h"
+
+// controller configuration parameters
+#define ADC_MAX_NUM_SAMPLES                     16
+#define ADC_SAMPLE_PERIOD_MS                    10
+#define ADC_VOLTAGE_ALARM_TH                    559
+#define ADC_VOLTAGE_ALARM_WINDOW                20
+#define ADC_VOLTAGE_ALARM_NOTE                  NOTE_LA
+#define ADC_VOLTAGE_ALARM_TIME_ON               30
+#define ADC_VOLTAGE_ALARM_TIME_OFF              30
+
+// communication configuration parameters
+#define DYN_MASTER_MAX_TX_BUFFER_LEN            128
+#define DYN_MASTER_MAX_RX_BUFFER_LEN            128
+#define DYN_MASTER_DEFAULT_BAUDRATE             1000000
+#define DYN_MASTER_DEFAULT_TIMEOUT_US           10000
+#define SERIAL_CONSOLE_MAX_BUFFER_LEN           128
+
+// motion configuration parameters
+#define MANAGER_MAX_NUM_SERVOS                  18
+#define MANAGER_MISSING_SERVOS_ALARM_NOTE       NOTE_SOL
+#define MANAGER_MISSING_SERVOS_ALARM_TIME_ON    10
+#define MANAGER_MISSING_SERVOS_ALARM_TIME_OFF   100
+#define BALANCE_GYRO_CAL_NUM_SAMPLES            10
+#define BALANCE_GYRO_X_CHANNEL                  3
+#define BALANCE_GYRO_Y_CHANNEL                  4
+#define BALANCE_GYRO_CAL_FAILED_ALARM_NOTE      NOTE_SOL
+#define BALANCE_GYRO_CAL_FAILED_ALARM_TIME_ON   10
+#define BALANCE_GYRO_CAL_FAILED_ALARM_TIME_OFF  100
+#define BALANCE_MAX_CAL_GYRO_ERROR              20.0
+#define BALANCE_FORWARD_FALL_THD_VALUE          (-200)
+#define BALANCE_BACKWARD_FALL_THD_VALUE         200
+#define BALANCE_KNEE_GAIN                       (4.0/54.0)
+#define BALANCE_ANKLE_ROLL_GAIN                 (4.0/18.0)
+#define BALANCE_HIP_PITCH_GAIN                  (4.0/20.0)
+#define BALANCE_ANKLE_PITCH_GAIN                (4.0/40.0)
+
+#endif
diff --git a/controllers/include/cont_cfg.h b/controllers/include/cont_cfg.h
new file mode 100644
index 0000000000000000000000000000000000000000..985b7b9a4e2abfc84c834cdcba1f249ac229ad84
--- /dev/null
+++ b/controllers/include/cont_cfg.h
@@ -0,0 +1,35 @@
+#ifndef _CONT_CFG_H
+#define _CONT_CFG_H
+
+#include "buzzer.h"
+
+// number of samples to average
+#ifndef ADC_MAX_NUM_SAMPLES
+  #define ADC_MAX_NUM_SAMPLES         16
+#endif
+
+#ifndef ADC_SAMPLE_PERIOD_MS
+  #define ADC_SAMPLE_PERIOD_MS        10
+#endif 
+
+#ifndef ADC_VOLTAGE_ALARM_TH
+  #define ADC_VOLTAGE_ALARM_TH        559
+#endif
+
+#ifndef ADC_VOLTAGE_ALARM_WINDOW      
+  #define ADC_VOLTAGE_ALARM_WINDOW    20
+#endif
+
+#ifndef ADC_VOLTAGE_ALARM_NOTE
+  #define ADC_VOLTAGE_ALARM_NOTE      NOTE_LA
+#endif
+
+#ifndef ADC_VOLTAGE_ALARM_TIME_ON
+  #define ADC_VOLTAGE_ALARM_TIME_ON   30
+#endif
+
+#ifndef ADC_VOLTAGE_ALARM_TIME_OFF
+  #define ADC_VOLTAGE_ALARM_TIME_OFF  30
+#endif
+
+#endif
diff --git a/controllers/include/user_time.h b/controllers/include/user_time.h
new file mode 100644
index 0000000000000000000000000000000000000000..ab080482202830d0982cb083051b1ee26e623483
--- /dev/null
+++ b/controllers/include/user_time.h
@@ -0,0 +1,15 @@
+#ifndef _USER_TIME_H
+#define _USER_TIME_H
+
+#include <avr/io.h>
+#include <avr/interrupt.h>
+
+/* public functions */
+void init_user_time(void);
+void user_time_set_period(uint16_t period_ms);
+void user_time_set_one_time(uint16_t time_ms);
+uint8_t user_time_is_period_done(void);
+uint8_t user_time_is_done(void);
+void user_time_stop(void);
+
+#endif
diff --git a/controllers/src/cm510/adc.c b/controllers/src/adc.c
similarity index 84%
rename from controllers/src/cm510/adc.c
rename to controllers/src/adc.c
index f5899c044773fe0b5acf9c8c5e0c9a4c3a3a8fc1..be4e5efc8944cddb57337f488729b31ff71b683d 100644
--- a/controllers/src/cm510/adc.c
+++ b/controllers/src/adc.c
@@ -1,5 +1,5 @@
+#include "cm510_cfg.h"
 #include "adc.h"
-#include "gpio.h"
 #include "buzzer.h"
 #include <util/delay.h>
 
@@ -14,6 +14,7 @@ volatile uint16_t adc_values[NUM_ADC];
 volatile uint16_t adc_avg_values[NUM_ADC];
 volatile uint8_t adc_current_ch;
 volatile uint8_t adc_current_sample;
+volatile uint8_t adc_voltage_alarm;
 
 /* private functions */
 void adc_set_channel(uint8_t ch_id)
@@ -94,15 +95,21 @@ void adc_loop(void)
       {
 //        voltage_mv=((uint32_t)data*(uint32_t)5000*(uint32_t)133)/((uint16_t)1024*(uint16_t)33);
         // compare with hysteresis
-        if(buzzer_is_playing_alarm())
+        if(adc_voltage_alarm==0x01)
         {
-          if(data>=(ADC_VOLTAGE_ALARM_TH+20))// voltage under 11 V
+          if(data>=(ADC_VOLTAGE_ALARM_TH+ADC_VOLTAGE_ALARM_WINDOW))// voltage under 11 V
+          {
             buzzer_stop_alarm();
+            adc_voltage_alarm=0x00;
+          }
         }
         else
         {
-          if(data<(ADC_VOLTAGE_ALARM_TH-20))// voltage under 11 V
+          if(data<(ADC_VOLTAGE_ALARM_TH-ADC_VOLTAGE_ALARM_WINDOW))// voltage under 11 V
+          {
             buzzer_start_alarm(NOTE_LA,30,30);
+            adc_voltage_alarm=0x01;
+          }
         }
       }
       adc_current_ch=(adc_current_ch+1)%NUM_ADC;
@@ -114,7 +121,7 @@ void adc_loop(void)
 /* public functions */
 void init_adc(void)
 {
-  uint8_t i=0;
+  uint8_t i=0,j=0;
 
   ADCSRA=(1 << ADEN) | (1 << ADPS2) | (1 << ADPS1) | (1 << ADPS0);// enable ADC set prescaler 1/128
                                                                   // disable auto trigger, interrupts disabled
@@ -127,9 +134,16 @@ void init_adc(void)
   DDRA  = 0xFC;
   PORTA = 0xFC;// all sensors disabled
 
+  // initialize internal sample variables to 12V to avoid initial voltage alarm
   for(i=0;i<NUM_ADC;i++)
-    adc_values[i]=0x0000;
+  {
+    adc_values[i]=0x0266;//equivalent to 12 V
+    adc_avg_values[i]=0x0266;//equivalent to 12 V
+    for(j=0;j<ADC_MAX_NUM_SAMPLES;j++)
+      adc_ch_data[i][j]=0x0266;
+  }
   adc_current_ch=0;
+  adc_voltage_alarm=0x00;
 
   // configure the timer 2 to perform periodic conversions (1 ms period)
   TCCR2A=(1<<WGM21);// CTC mode, no output
diff --git a/controllers/src/cm510/buzzer.c b/controllers/src/buzzer.c
similarity index 90%
rename from controllers/src/cm510/buzzer.c
rename to controllers/src/buzzer.c
index 0222db063d5f7ac3b3d83c9ed4baec120118e8d9..115cd534a008eb8e8198e3d11f7cd0465bee8f8b 100644
--- a/controllers/src/cm510/buzzer.c
+++ b/controllers/src/buzzer.c
@@ -68,22 +68,6 @@ void buzzer_loop(void)
   }
 }
 
-void buzzer_start_alarm(note_t note, uint16_t on_time_100ms,uint16_t off_time_100ms)
-{
-  if(buzzer_playing_alarm==0x00)// the alram is not playing
-  {
-    if(buzzer_playing)// if the normal buzzer is playing, stop it.
-      buzzer_stop();
-    buzzer_playing_alarm=0x01;
-    buzzer_time_on_100ms=on_time_100ms;
-    buzzer_time_off_100ms=off_time_100ms;
-    buzzer_note=note;
-    // enable PWM output
-    TCCR1A&=(~(1<<COM1A0));
-    TCCR1A|=(1<<COM1A1);
-  }
-}
-
 void buzzer_stop_alarm(void)
 {
   if(buzzer_playing_alarm==0x01)
@@ -94,6 +78,27 @@ void buzzer_stop_alarm(void)
   }
 }
 
+void buzzer_start_alarm(note_t note, uint16_t on_time_100ms,uint16_t off_time_100ms)
+{
+  if(buzzer_playing_alarm)// the alram is not playing
+    buzzer_stop_alarm();
+  else if(buzzer_playing)// if the normal buzzer is playing, stop it.
+    buzzer_stop();
+  // set top count value
+  ICR1H = 0x00;
+  ICR1L = buzzer_note_freq[note];
+  // Set the compare value to half the period
+  OCR1AH = 0x00;
+  OCR1AL = buzzer_note_freq[note]>>1;
+  buzzer_time_on_100ms=on_time_100ms;
+  buzzer_time_off_100ms=off_time_100ms;
+  buzzer_note=note;
+  // enable PWM output
+  TCCR1A&=(~(1<<COM1A0));
+  TCCR1A|=(1<<COM1A1);
+  buzzer_playing_alarm=0x01;
+}
+
 /* public functions */
 void init_buzzer(void)
 {
diff --git a/controllers/src/cm510/cm510.c b/controllers/src/cm510.c
similarity index 83%
rename from controllers/src/cm510/cm510.c
rename to controllers/src/cm510.c
index 0b25730059316d7fff64f3498df39de48bba5edf..316ab01dded22f064b6e417654f27ecc27bafcc7 100755
--- a/controllers/src/cm510/cm510.c
+++ b/controllers/src/cm510.c
@@ -15,14 +15,12 @@
  *  along with iri-libbioloid.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include "cm510.h"
-#include "gpio.h"
-#include "adc.h"
-#include "buzzer.h"
 #include <avr/io.h>
 #include <avr/interrupt.h>
 #include <inttypes.h>
 #include <util/delay.h>
+#include "cm510_cfg.h"
+#include "cm510.h"
 
 /* external user functions */
 extern void user_init(void);
@@ -32,6 +30,8 @@ extern void user_loop(void);
 extern void pushbuttons_loop(void);
 extern void adc_loop(void);
 extern void buzzer_loop(void);
+extern void manager_loop(void);
+extern void user_time_loop(void);
 
 // general interface
 void init_cm510(void)
@@ -40,20 +40,26 @@ void init_cm510(void)
   init_buttons();
   init_adc();
   init_buzzer();
+  init_user_time();
 }
 
 int16_t main(void)
 {
   init_cm510();
+  sei();
+  manager_init(MANAGER_MAX_NUM_SERVOS);
   /* call the user initialization function */
   user_init();
-  sei();
+  // turn BAT_LED on to indicate the initialization is done
+  turn_led_on(LED_BAT);
 
   while(1)
   {
     pushbuttons_loop();
     adc_loop();
     buzzer_loop();
+    manager_loop();
+    user_time_loop();
     user_loop();
   }
 }
diff --git a/controllers/src/examples/Makefile b/controllers/src/examples/Makefile
index fcb8394727c000908ba201b6c99d3beecbc45515..db83592ea8c4d3501365f3b0ed1bd8bc74b83a87 100644
--- a/controllers/src/examples/Makefile
+++ b/controllers/src/examples/Makefile
@@ -6,15 +6,17 @@ SOURCES=main.c
 
 OBJS=$(SOURCES:.c=.o)
 SRC_DIR=./src/
-CM510_DIR=../../lib
-COMM_DIR=../../../communications/lib
+DEV_DIR=../../../dyn_devices/
+COMM_DIR=../../../communications/
+MAN_DIR=../../../motion/
+CONT_DIR=../../
 CC=avr-gcc
 OBJCOPY=avr-objcopy
 MMCU=atmega2561
 
-INCLUDE_DIRS=-I../../include/ -I../../../communications/include
+INCLUDE_DIRS=-I$(CONT_DIR)include/ -I$(COMM_DIR)include -I$(MAN_DIR)include -I$(DEV_DIR)include
 
-LIBS=$(CM510_DIR)/libcontrollers.a $(COMM_DIR)/libcomm.a
+LIBS=$(CONT_DIR)lib/libcontrollers.a $(MAN_DIR)lib/libmotion_manager.a $(COMM_DIR)lib/libcomm.a $(DEV_DIR)lib/libdyn_devices.a
 
 CFLAGS=-mmcu=$(MMCU) -Wall -Os $(defines) -DF_CPU=16000000UL -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
 
diff --git a/controllers/src/examples/main.c b/controllers/src/examples/main.c
index a3a9ce57bc59b46b152cf0bff56b32bcd989ecf1..4100e66b48115655465d7626ab0504796eedee72 100755
--- a/controllers/src/examples/main.c
+++ b/controllers/src/examples/main.c
@@ -20,24 +20,27 @@
 #include "adc.h"
 #include "buzzer.h"
 #include "serial_console.h"
+#include "user_time.h"
 #include <stdio.h>
 #include <util/delay.h>
 
 void user_init(void)
 {
   serial_console_init(57600);
+  user_time_set_period(10);
 }
 
 void user_loop(void)
 {
+  if(user_time_is_period_done())
+  {
+    printf("Gyro X: %d\n",get_adc_avg_channel(ADC_PORT_3));
+    printf("Gyro Y: %d\n",get_adc_avg_channel(ADC_PORT_4));
+  }
   if(is_button_pressed(BTN_START))
     turn_led_on(LED_AUX);
   else
     turn_led_off(LED_AUX);
-//  if(get_adc_avg_channel(ADC_VCC)<512)
-//    turn_led_on(LED_PLAY);
-//  else
-//    turn_led_off(LED_PLAY);
   if(is_button_falling_edge(BTN_UP))
     buzzer_start(NOTE_SOL,30,30);
   else
@@ -45,5 +48,4 @@ void user_loop(void)
     if(is_button_falling_edge(BTN_DOWN))
       buzzer_stop();
   }
-  printf("loop\n");
 }
diff --git a/controllers/src/cm510/gpio.c b/controllers/src/gpio.c
similarity index 100%
rename from controllers/src/cm510/gpio.c
rename to controllers/src/gpio.c
diff --git a/controllers/src/user_time.c b/controllers/src/user_time.c
new file mode 100644
index 0000000000000000000000000000000000000000..90a29a408040df38c986d49bd3ea1a76e0badb42
--- /dev/null
+++ b/controllers/src/user_time.c
@@ -0,0 +1,126 @@
+#include "user_time.h"
+
+/* private variables */
+uint8_t user_time_single;
+uint16_t user_time_time;
+uint16_t user_time_period;
+volatile uint8_t user_time_done;
+volatile uint8_t user_time_period_done;
+
+/* private funcitons */
+void user_time_loop(void)
+{
+  static uint16_t count=0;
+
+  if(TIFR5&(1<<OCF5A))
+  {
+    TIFR5|=(1<<OCF5A);// clear any interrupt
+    TCNT5H=0x00;                   
+    TCNT5L=0x00;
+    if(user_time_single==0x01)
+    {
+      if(count==user_time_time)
+      {
+        count=0;
+        user_time_done=0x01;
+        TCCR5B&=0xF8;// disable timer 
+      }
+      else
+        count++;
+    }
+    else
+    {
+      if(count==user_time_period)
+      {
+        count=0;
+        user_time_period_done=0x01;
+      }
+      else
+        count++;
+    }
+  }
+}
+
+/* public functions */
+void init_user_time(void)
+{
+  // initialize timer3 for buzzer time control (100ms period)
+  TIMSK5=0x00;// disable all interrupts
+  TIFR5=0x2F;// clear any pending interrupt
+  TCNT5H=0x00;                   
+  TCNT5L=0x00;
+  // set PWM mode with ICR top-count (CTC mode)
+  TCCR5A=0x00;
+  TCCR5A&=~(1<<WGM50);
+  TCCR5A&=~(1<<WGM51);
+  TCCR5B=0x00;
+  TCCR5B|=(1<<WGM52);
+  TCCR5B&=~(1<<WGM53);
+  // clear output compare value A
+  OCR5AH=0x18;
+  OCR5AL=0x6A;
+  TCCR5B&=0xF8;// disable timer by default
+
+  /* initialize internal variables */
+  user_time_single=0x00;
+  user_time_time=0x00;
+  user_time_period=0x00;
+  user_time_done=0x00;
+  user_time_period_done=0x00;
+}
+
+void user_time_set_period(uint16_t period_ms)
+{
+  user_time_single=0x00;
+  user_time_period=period_ms/100;
+  user_time_period_done=0x00;
+  TCNT5H=0x00;                   
+  TCNT5L=0x00;
+  TIFR5=0x2F;// clear any pending interrupt
+  TCCR5B&=0xF8;// disable timer by default
+  TCCR5B|=0x04;
+}
+
+void user_time_set_one_time(uint16_t time_ms)
+{
+  user_time_single=0x01;
+  user_time_time=time_ms/100;
+  user_time_done=0x00;
+  TCNT5H=0x00;                   
+  TCNT5L=0x00;
+  TIFR5=0x2F;// clear any pending interrupt
+  TCCR5B&=0xF8;// disable timer by default
+  TCCR5B|=0x04;
+}
+
+uint8_t user_time_is_period_done(void)
+{
+  if(user_time_period_done==0x01)
+  {
+    user_time_period_done=0x00;
+    return 0x01;
+  }
+  else
+    return 0x00;
+}
+
+uint8_t user_time_is_done(void)
+{
+  if(user_time_done==0x01)
+  {
+    user_time_done=0x00;
+    return 0x01;
+  }
+  else
+    return 0x00;
+}
+
+void user_time_stop(void)
+{
+  TCCR5B&=0xF8;// disable timer 
+  TIFR5|=(1<<OCF5A);// clear any interrupt
+  TCNT5H=0x00;                   
+  TCNT5L=0x00;
+  user_time_period_done=0x00;  
+  user_time_done=0x00;  
+}
diff --git a/dyn_devices/Makefile b/dyn_devices/Makefile
index b7af2ae6f9ff4695a5df146b5336387cc0a1bb0f..44b6332e1d6be66947d0201478045048d607c92e 100755
--- a/dyn_devices/Makefile
+++ b/dyn_devices/Makefile
@@ -19,7 +19,7 @@ ARFLAGS=rsc
 
 .PHONY: all show_banner clean
 
-all: show_banner $(PROJECT).a examples
+all: show_banner communications $(PROJECT).a 
 
 show_banner:
 	@echo "------------------------------------------------------";
@@ -34,11 +34,14 @@ show_banner:
 	@echo "------------------------------------------------------";
 
 $(PROJECT).a: ${OBJS}
-	$(OBJCOPY) $(ARFLAGS) ${LIB_DIR}$(PROJECT).a $(LIBS) $(OBJS)
+	$(OBJCOPY) $(ARFLAGS) ${LIB_DIR}$(PROJECT).a $(OBJS)
 
 %.o: %.c 
 	$(CC) -c $(CFLAGS) $(INC_DIRS) -o $@ $<
 
+communications:
+	$(MAKE) -C $(COMM_DIR)
+
 examples:
 	$(MAKE) -C src/examples 
 
diff --git a/dyn_devices/src/examples/Makefile b/dyn_devices/src/examples/Makefile
index f5be84ed36e5399b332a24ba6b8abab7ce157c44..e47489236aa81efc8ff2c3bd7ba6d599ae7006d5 100755
--- a/dyn_devices/src/examples/Makefile
+++ b/dyn_devices/src/examples/Makefile
@@ -24,15 +24,21 @@ HEX_FLASH_FLAGS = -R .eeprom -R .fuse -R .lock -R .signature
 
 .PHONY: all
 
-all: $(PROJECT).hex
+all: communications dyn_devices $(PROJECT).hex
 
 $(PROJECT).hex: $(PROJECT).elf
 	$(OBJCOPY) -O ihex $(HEX_FLASH_FLAGS)  $< $@
-$(PROJECT).elf: $(OBJS)
+$(PROJECT).elf: $(OBJS) $(COMM_DIR)lib/libcomm.a $(DEV_DIR)lib/libdyn_devices.a
 	$(CC) $(LDFLAGS) $(OBJS) $(LIB_DIRS) $(LIBS) -o $(PROJECT).elf
 %.o:%.c
 	$(CC) -c $(CFLAGS) $(INCLUDE_DIRS) -o $@ $<
 
+communications:
+	$(MAKE) -C $(COMM_DIR)
+
+dyn_devices:
+	$(MAKE) -C $(DEV_DIR)
+
 download: $(MAIN_OUT_HEX)
 	fw_downloader -d /dev/ttyUSB0 -f ./src/examples/comm_ex.hex -p new
 
diff --git a/motion/Makefile b/motion/Makefile
index c89f25848cd2d3d9898e1843e3b967c9f73ba641..a6372baf5b0addcfbf95247cdd79507d8e64509a 100755
--- a/motion/Makefile
+++ b/motion/Makefile
@@ -1,18 +1,19 @@
 PROJECT=libmotion_manager
-SOURCES=src/motion_manager.c src/action.c src/motion_pages.c
+SOURCES=src/motion_manager.c src/action.c src/motion_pages.c src/balance.c
 OBJS=$(SOURCES:.c=.o)
 SRC_DIR=./src/ 
 BIN_DIR=./build/
 LIB_DIR=./lib/
 COMM_DIR=../communications/
 DEV_DIR=../dyn_devices/
+CONT_DIR=../controllers/
 CC=avr-gcc
 OBJCOPY=avr-ar
 MMCU=atmega2561
 
-INC_DIRS=-I./include/ -I$(DEV_DIR)include/ -I$(COMM_DIR)include/
+INC_DIRS=-I./include/ -I$(DEV_DIR)include/ -I$(COMM_DIR)include/ -I$(CONT_DIR)include/
 
-LIBS=$(DEV_DIR)lib/libdyn_devices.a $(COMM_DIR)lib/libcomm.a 
+LIBS=$(COMM_DIR)lib/libcomm.a $(DEV_DIR)lib/libdyn_devices.a
 
 CFLAGS=-mmcu=$(MMCU) -Wall -O3 -DF_CPU=16000000UL -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
 
@@ -20,7 +21,7 @@ ARFLAGS= rsc
 
 .PHONY: all show_banner clean
 
-all: show_banner $(PROJECT).a
+all: show_banner communications dyn_devices $(PROJECT).a
 
 show_banner:
 	@echo "------------------------------------------------------";
@@ -36,11 +37,21 @@ show_banner:
 
 
 $(PROJECT).a: ${OBJS}
-	$(OBJCOPY) $(ARFLAGS) ${LDIR}$(PROJECT).a $(LIBS) $(OBJS)
+	$(OBJCOPY) $(ARFLAGS) ${LIB_DIR}$(PROJECT).a $(OBJS)
+
+communications:
+	$(MAKE) -C $(COMM_DIR)
+
+dyn_devices:
+	$(MAKE) -C $(DEV_DIR)
+
+examples:
+	$(MAKE) -C src/examples
 
 %.o: %.c 
 	$(CC) -c $(CFLAGS) ${INC_DIRS} -o $@ $<
 
 clean:
-	rm -f ${LDIR}$(PROJECT).a
+	rm -f ${LIB_DIR}$(PROJECT).a
 	rm -f $(OBJS) 
+	$(MAKE) -C src/examples clean
diff --git a/motion/include/action.h b/motion/include/action.h
index 549728d6745d48b1e7eaeb32fab03d9045a59000..929ad3fa569e85a042af698bd77a7e1e168f24ec 100644
--- a/motion/include/action.h
+++ b/motion/include/action.h
@@ -1,18 +1,10 @@
 #ifndef _ACTION_H
 #define _ACTION_H
 
-#include <inttypes.h>
-
-#define NUM_SERVOS 	18
-
 // basic motion interface
-void action_init(void);
 uint8_t action_set_page(uint8_t page_id);
 void action_start_page(void);
 void action_stop_page(void);
 uint8_t is_action_running(void);
 
-// motion manager interface
-void action_process(void);
-
 #endif
diff --git a/motion/include/balance.h b/motion/include/balance.h
new file mode 100644
index 0000000000000000000000000000000000000000..5c227eb00a2be42b38b8fcdd38541ed20935aced
--- /dev/null
+++ b/motion/include/balance.h
@@ -0,0 +1,50 @@
+#ifndef _BALANCE_H
+#define _BALANCE_H
+
+typedef enum {robot_standing,robot_face_down,robot_face_up} fallen_t;
+
+// public functions
+void balance_init(void);
+/**
+ * \brief Function to check if the robot has fallen to the ground
+ * 
+ * This function returns:
+ *   0x00 : if the robot is standing.
+ *   0x01 : if the robot has fallen face-to-ground 
+ *   0x02 : if the robot has fallen back-to-ground
+ * After reading the value, the internal state is automatically set
+ * back to 0x00
+ */
+fallen_t balance_robot_has_fallen(void);
+uint8_t balance_calibrate_gyro(void);
+/**
+ * \brief Function to enable the internal gyros
+ * 
+ * If the gyroscopes have been detected and calibrated in the init_motion() function,
+ * this function enbales them to be used. When enabled they perform two tasks: first
+ * balance the robot while moving to make it more stable and also detect when the 
+ * robot falls.
+ */
+void balance_enable_gyro(void);
+/**
+ * \brief Function to disable the internal gyros
+ *
+ * If the gyroscopes have been detected and calibrated in the init_motion() function,
+ * this function disales them. When disabled it is no longer possible to detect when 
+ * robot falls. gyro
+ *
+ * This function may be used to avoid cancelling the standing up action due to fast 
+ * motions associated with this motion. After the robot is standing up, the gyros may
+ * be enabled again.
+ */
+void balance_disable_gyro(void);
+/**
+ * \brief Function to check whether the gyros are enabled or not
+ *
+ * This function checks wether the gyros are currently enabled or not.
+ *
+ * \return 0x01 if the gyros are enabled and 0x00 otherwise.
+ */
+uint8_t balance_is_gyro_enabled(void);
+
+#endif
diff --git a/motion/include/motion_cfg.h b/motion/include/motion_cfg.h
new file mode 100644
index 0000000000000000000000000000000000000000..f96994c4cfcd0624ab272636e478ffeceefb09dd
--- /dev/null
+++ b/motion/include/motion_cfg.h
@@ -0,0 +1,74 @@
+#ifndef _MOTION_CFG_H
+#define _MOTION_CFG_H
+
+#include "buzzer.h"
+
+#ifndef MANAGER_MAX_NUM_SERVOS
+  #define MANAGER_MAX_NUM_SERVOS                  18
+#endif
+
+#ifndef MANAGER_MISSING_SERVOS_ALARM_NOTE
+  #define MANAGER_MISSING_SERVOS_ALARM_NOTE       NOTE_SOL
+#endif
+
+#ifndef MANAGER_MISSING_SERVOS_ALARM_TIME_ON
+  #define MANAGER_MISSING_SERVOS_ALARM_TIME_ON    10
+#endif
+
+#ifndef MANAGER_MISSING_SERVOS_ALARM_TIME_OFF
+  #define MANAGER_MISSING_SERVOS_ALARM_TIME_OFF   100
+#endif
+
+#ifndef BALANCE_GYRO_CAL_NUM_SAMPLES
+  #define BALANCE_GYRO_CAL_NUM_SAMPLES            10
+#endif 
+
+#ifndef BALANCE_GYRO_X_CHANNHEL
+  #define BALANCE_GYRO_X_CHANNEL                  3
+#endif 
+
+#ifndef BALANCE_GYRO_Y_CHANNEL
+  #define BALANCE_GYRO_Y_CHANNEL                  4
+#endif
+
+#ifndef BALANCE_GYRO_CAL_FAILED_ALARM_NOTE
+  #define BALANCE_GYRO_CAL_FAILED_ALARM_NOTE      NOTE_SOL
+#endif
+
+#ifndef BALANCE_GYRO_CAL_FAILED_ALARM_TIME_ON
+  #define BALANCE_GYRO_CAL_FAILED_ALARM_TIME_ON   10
+#endif
+
+#ifndef BALANCE_GYRO_CAL_FAILED_ALARM_TIME_OFF
+  #define BALANCE_GYRO_CAL_FAILED_ALARM_TIME_OFF  100
+#endif
+
+#ifndef BALANCE_MAX_CAL_GYRO_ERROR
+  #define BALANCE_MAX_CAL_GYRO_ERROR              20.0
+#endif
+
+#ifndef BALANCE_FORWARD_FALL_THD_VALUE
+  #define BALANCE_FORWARD_FALL_THD_VALUE          (-200)
+#endif
+
+#ifndef BALANCE_BACKWARD_FALL_THD_VALUE
+  #define BALANCE_BACKWARD_FALL_THD_VALUE         200
+#endif
+
+#ifndef BALANCE_KNEE_GAIN
+  #define BALANCE_KNEE_GAIN                       (4.0/54.0)
+#endif
+
+#ifndef BALANCE_ANKLE_ROLL_GAIN
+  #define BALANCE_ANKLE_ROLL_GAIN                 (4.0/18.0)
+#endif
+
+#ifndef BALANCE_HIP_PITCH_GAIN
+  #define BALANCE_HIP_PITCH_GAIN                  (4.0/20.0)
+#endif
+
+#ifndef BALANCE_ANKLE_PITCH_GAIN
+  #define BALANCE_ANKLE_PITCH_GAIN                (4.0/40.0)
+#endif
+
+#endif
diff --git a/motion/include/motion_manager.h b/motion/include/motion_manager.h
index 3a1ea88bdb7c748a4bb66fc45b51b313b1a4f192..29b0ee61b9c8905828b823f6c75135ba69be8682 100644
--- a/motion/include/motion_manager.h
+++ b/motion/include/motion_manager.h
@@ -2,15 +2,6 @@
 #define _MOTION_MANAGER_H
 
 #include <inttypes.h>
-#include "motion_pages.h"
-
-#define       MANAGER_MAX_NUM_SERVOS         20
-#define       GYRO_CAL_NUM_SAMPLES           10
-#define       X_GYRO_CHANNEL                  3
-#define       Y_GYRO_CHANNEL                  4
-
-extern TPage action_next_page;
-extern TPage action_current_page;
 
 // servo information structure
 typedef struct{
@@ -23,57 +14,7 @@ typedef struct{
   uint8_t ccw_slope;
 }TServoInfo;
 
-int16_t servo_offsets[MANAGER_MAX_NUM_SERVOS];
-
 // public functions
-void manager_init(uint8_t num_servos);
-void manager_set_index_value(uint8_t servo_id, uint16_t value);
-void manager_set_servo_value(uint8_t servo_id, uint16_t value);
-uint16_t manager_get_index_value(uint8_t servo_id);
-uint16_t manager_get_servo_value(uint8_t servo_id);
-void manager_set_servo_slopes(uint8_t servo_id, uint8_t slope);
-
-/**
- * \brief Function to check if the robot has fallen to the ground
- * 
- * This function returns:
- *   0x00 : if the robot is standing.
- *   0x01 : if the robot has fallen face-to-ground 
- *   0x02 : if the robot has fallen back-to-ground
- * After reading the value, the internal state is automatically set
- * back to 0x00
- */
-uint8_t manager_check_robot_fallen(void);
+uint8_t manager_init(uint8_t num_servos);
 
-// gyro interface
-uint8_t manager_calibrate_gyro(void);
-/**
- * \brief Function to enable the internal gyros
- * 
- * If the gyroscopes have been detected and calibrated in the init_motion() function,
- * this function enbales them to be used. When enabled they perform two tasks: first
- * balance the robot while moving to make it more stable and also detect when the 
- * robot falls.
- */
-void manager_enable_gyro(void);
-/**
- * \brief Function to disable the internal gyros
- *
- * If the gyroscopes have been detected and calibrated in the init_motion() function,
- * this function disales them. When disabled it is no longer possible to detect when 
- * robot falls. gyro
- *
- * This function may be used to avoid cancelling the standing up action due to fast 
- * motions associated with this motion. After the robot is standing up, the gyros may
- * be enabled again.
- */
-void manager_disable_gyro(void);
-/**
- * \brief Function to check whether the gyros are enabled or not
- *
- * This function checks wether the gyros are currently enabled or not.
- *
- * \return 0x01 if the gyros are enabled and 0x00 otherwise.
- */
-uint8_t manager_is_gyro_enabled(void);
 #endif
diff --git a/motion/src/action.c b/motion/src/action.c
index 4cd742e9d92ddf8dd892d85b7dff595a37388967..5452f9ad1b777c6a1dc7959ae7c1617f14a7a46b 100644
--- a/motion/src/action.c
+++ b/motion/src/action.c
@@ -1,12 +1,9 @@
-#include "action.h"
-#include "dyn_servos.h"
-#include "motion_pages.h"
-#include "motion_manager.h"
 #include <avr/io.h>
 #include <avr/interrupt.h>
 #include <util/delay.h>
-#include "cm510.h"
-
+#include "motion_cfg.h"
+#include "action.h"
+#include "motion_pages.h"
 
 /**************************************
  * Section             /----\
@@ -23,6 +20,12 @@ typedef enum{ ZERO_FINISH, NONE_ZERO_FINISH} TStopMode;
 #define       TIME_BASE_SCHEDULE       0x0a
 #define       INVALID_BIT_MASK         0x4000
 
+// external functions
+extern uint16_t manager_get_index_value(uint8_t servo_id);
+extern uint16_t manager_get_servo_value(uint8_t servo_id);
+extern void manager_set_servo_value(uint8_t servo_id, uint16_t value);
+extern void manager_set_servo_slopes(uint8_t servo_id, uint8_t slope);
+
 // private variables
 uint8_t action_finished;
 uint8_t action_stop;
@@ -71,10 +74,7 @@ uint8_t action_set_page(uint8_t page_id)
 
   error=load_page_info(page_id,&action_current_page);
   if(error==0)
-  {
-    //ram_write_byte(CM730_ACTION_NUMBER,page_id);
     action_current_index=page_id;
-  }
   return error;
 }
 
@@ -90,9 +90,8 @@ void action_start_page(void)
   action_step_count = 0;
   bPlayRepeatCount = action_current_page.header.repeat;
   action_next_index = 0x00;
-  
 
-  for( i=1; i<=NUM_SERVOS; i++ )
+  for( i=1; i<=MANAGER_MAX_NUM_SERVOS; i++ )
   {
       wpTargetAngle1024[i] = manager_get_index_value(i);
       ipLastOutSpeed1024[i] = 0;
@@ -100,16 +99,11 @@ void action_start_page(void)
       ipGoalSpeed1024[i] = 0;
   }
   action_is_running=0x01;
-  //ram_write_byte(CM730_ACTION_STATUS,0x01);
-  //ram_set_bit(CM730_ACTION_CONTROL,0x00);
-  //ram_clear_bit(CM730_ACTION_CONTROL,0x01);
 }
 
 void action_stop_page(void)
 {
   action_stop=0x01;
-  //ram_clear_bit(CM730_ACTION_CONTROL,0x00);
-  //ram_set_bit(CM730_ACTION_CONTROL,0x01);
 }
 
 uint8_t is_action_running(void)
@@ -132,7 +126,6 @@ void action_process(void)
   int64_t lStartSpeed1024_PreTime_256T;
   int64_t lMovingAngle_Speed1024Scale_256T_2T;
   int64_t lDivider1,lDivider2;
-  uint16_t wMaxAngle1024;
   uint16_t wMaxSpeed256;
   uint16_t wPrevTargetAngle; // Start position
   uint16_t wCurrentTargetAngle; // Target position
@@ -147,7 +140,7 @@ void action_process(void)
   {
     wUnitTimeCount = 0;
 
-    for(bID=1;bID<=NUM_SERVOS;bID++)
+    for(bID=1;bID<=MANAGER_MAX_NUM_SERVOS;bID++)
     {
       wpStartAngle1024[bID] = manager_get_servo_value(bID);
       ipLastOutSpeed1024[bID] = ipGoalSpeed1024[bID];
@@ -158,9 +151,9 @@ void action_process(void)
     {
       // MAIN Section Áغñ
       bSection = MAIN_SECTION;
-      turn_led_off(LED_TxD);
+//      turn_led_off(LED_TxD);
       wUnitTimeNum =  wUnitTimeTotalNum - (wAccelStep << 1);
-      for(bID=1;bID<=NUM_SERVOS;bID++)
+      for(bID=1;bID<=MANAGER_MAX_NUM_SERVOS;bID++)
       {
         if( bpFinishType[bID] == NONE_ZERO_FINISH )
         {
@@ -177,10 +170,10 @@ void action_process(void)
     {
       // POST Section Áغñ
       bSection = POST_SECTION;
-      turn_led_off(LED_TxD);
+//      turn_led_off(LED_TxD);
       wUnitTimeNum = wAccelStep;
 
-      for(bID=1;bID<=NUM_SERVOS;bID++)
+      for(bID=1;bID<=MANAGER_MAX_NUM_SERVOS;bID++)
       {
         ipMainAngle1024[bID] = ipMovingAngle1024[bID] - ipMainAngle1024[bID] - ipAccelAngle1024[bID];
       }
@@ -191,22 +184,22 @@ void action_process(void)
       if( wPauseTime )
       {
         bSection = PAUSE_SECTION;
-        turn_led_off(LED_TxD);
+//        turn_led_off(LED_TxD);
         wUnitTimeNum = wPauseTime;
       }
       else
       {
         bSection = PRE_SECTION;
-        turn_led_on(LED_TxD);
+//        turn_led_on(LED_TxD);
       }
     }
     else if( bSection == PAUSE_SECTION )
     {
       // PRE Section Áغñ
       bSection = PRE_SECTION;
-      turn_led_on(LED_TxD);
+//      turn_led_on(LED_TxD);
 
-      for(bID=1;bID<=NUM_SERVOS;bID++)
+      for(bID=1;bID<=MANAGER_MAX_NUM_SERVOS;bID++)
       {
         ipLastOutSpeed1024[bID] = 0;
       }
@@ -273,7 +266,7 @@ void action_process(void)
         //  wMaxAngle1024 = 0;
 
       ////////// Jointº° ÆĶó¹ÌÅÍ °è»ê
-      for(bID=1;bID<=NUM_SERVOS;bID++)
+      for(bID=1;bID<=MANAGER_MAX_NUM_SERVOS;bID++)
       {
           // ÀÌÀü, ÇöÀç, ¹Ì·¡¸¦ ¹ÙÅÁÀ¸·Î ±ËÀûÀ» °è»ê
           ipAccelAngle1024[bID] = 0;
@@ -366,7 +359,7 @@ void action_process(void)
 
       if(lDivider2 == 0)
         lDivider2 = 1;
-      for(bID=1;bID<=NUM_SERVOS;bID++)
+      for(bID=1;bID<=MANAGER_MAX_NUM_SERVOS;bID++)
       {
           lStartSpeed1024_PreTime_256T = (long)ipLastOutSpeed1024[bID] * ulPreSectionTime256T; //  *300/1024 * 1024/720 * 256 * 2
           lMovingAngle_Speed1024Scale_256T_2T = (((long)ipMovingAngle1024[bID]) * 2560L) / 12;
@@ -389,11 +382,11 @@ void action_process(void)
     wUnitTimeCount++;
     if( bSection == PAUSE_SECTION )
     {
-      toggle_led(LED_AUX);
+//      toggle_led(LED_AUX);
     }
     else
     {
-      for(bID=1;bID<=NUM_SERVOS;bID++)
+      for(bID=1;bID<=MANAGER_MAX_NUM_SERVOS;bID++)
       {
         if( ipMovingAngle1024[bID] == 0 )
           manager_set_servo_value(bID,wpStartAngle1024[bID]);
diff --git a/motion/src/balance.c b/motion/src/balance.c
new file mode 100644
index 0000000000000000000000000000000000000000..d7461ef0fca2bfb56269050ca5a558a5045ec857
--- /dev/null
+++ b/motion/src/balance.c
@@ -0,0 +1,141 @@
+#include <avr/io.h>
+#include <avr/interrupt.h>
+#include <util/delay.h>
+#include "motion_cfg.h"
+#include "balance.h"
+#include "adc.h"
+#include "buzzer.h"
+
+// external functions
+extern void buzzer_start_alarm(note_t note,uint16_t on_time_100ms,uint16_t off_time_100ms);
+extern void buzzer_stop_alarm(void);
+
+// private variables
+uint16_t balance_x_gyro_center;
+uint16_t balance_y_gyro_center;
+uint8_t balance_gyro_calibrated;
+uint8_t balance_enabled;
+uint8_t balance_robot_fallen_state;
+int16_t balance_offsets[MANAGER_MAX_NUM_SERVOS];
+
+// private functions
+void balance_loop(void)
+{
+  int16_t gyro_x=0;
+  int16_t gyro_y=0;
+
+  if(balance_enabled == 0x01 && balance_gyro_calibrated == 0x01)
+  {
+    // get x,y gyroscope
+    gyro_x=get_adc_channel(BALANCE_GYRO_X_CHANNEL)-balance_x_gyro_center;
+    gyro_y=get_adc_channel(BALANCE_GYRO_Y_CHANNEL)-balance_y_gyro_center;
+
+    if(gyro_x > BALANCE_BACKWARD_FALL_THD_VALUE)
+      balance_robot_fallen_state = robot_face_up;
+    else if(gyro_x < BALANCE_FORWARD_FALL_THD_VALUE)
+      balance_robot_fallen_state = robot_face_down;
+
+    float x_error1,x_error2,y_error1,y_error2;
+    x_error1=gyro_x*BALANCE_KNEE_GAIN;//4.0/54.0;
+    x_error2=gyro_x*BALANCE_ANKLE_ROLL_GAIN;//4.0/18.0;
+    y_error1=gyro_y*BALANCE_HIP_PITCH_GAIN;//4.0/20.0;
+    y_error2=gyro_y*BALANCE_ANKLE_PITCH_GAIN;//4.0/40.0;
+
+    balance_offsets[8] = (uint16_t)y_error1;
+    balance_offsets[9] = (uint16_t)y_error1;
+    balance_offsets[12] = (uint16_t)x_error1;//-1.0;
+    balance_offsets[13] = (uint16_t)-x_error1;//+1.0;
+    balance_offsets[14] = (uint16_t)+x_error2;
+    balance_offsets[15] = (uint16_t)-x_error2;
+    balance_offsets[16] = (uint16_t)-y_error2;
+    balance_offsets[17] = (uint16_t)-y_error2;
+  }
+}
+
+int16_t balance_get_offset(uint8_t index)
+{
+  return balance_offsets[index];
+}
+
+void balance_get_all_offsets(int16_t **offsets)
+{
+  *offsets=&balance_offsets[0];
+}
+
+// public functions
+void balance_init(void)
+{
+  uint8_t i;
+
+  balance_x_gyro_center=0;
+  balance_y_gyro_center=0;
+  balance_gyro_calibrated=0x00;
+  balance_enabled=0x00;
+  balance_robot_fallen_state=robot_standing;
+  for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
+    balance_offsets[i]=0;
+}
+
+uint8_t balance_robot_has_fallen(void)
+{
+  uint8_t state = balance_robot_fallen_state;
+  balance_robot_fallen_state = robot_standing;
+  return state;
+}
+
+uint8_t balance_calibrate_gyro(void)
+{
+  uint16_t x_gyro_values[BALANCE_GYRO_CAL_NUM_SAMPLES];
+  uint16_t y_gyro_values[BALANCE_GYRO_CAL_NUM_SAMPLES];
+  float x_gyro_average=0.0,y_gyro_average=0.0,std_x=0.0,std_y=0.0;
+  uint8_t i=0;
+
+  balance_x_gyro_center=0;
+  balance_y_gyro_center=0;
+  for(i=0;i<BALANCE_GYRO_CAL_NUM_SAMPLES;i++)
+  {
+    _delay_ms(50);
+    x_gyro_values[i]=get_adc_channel(BALANCE_GYRO_X_CHANNEL); 
+    y_gyro_values[i]=get_adc_channel(BALANCE_GYRO_Y_CHANNEL); 
+    x_gyro_average+=x_gyro_values[i];
+    y_gyro_average+=y_gyro_values[i];
+  }  
+  x_gyro_average/=(float)BALANCE_GYRO_CAL_NUM_SAMPLES;
+  y_gyro_average/=(float)BALANCE_GYRO_CAL_NUM_SAMPLES;
+  // compute the standard deviation
+  for(i=0;i<BALANCE_GYRO_CAL_NUM_SAMPLES;i++)
+  {
+    std_x+=(x_gyro_values[i]-x_gyro_average)*(x_gyro_values[i]-x_gyro_average);
+    std_y+=(y_gyro_values[i]-y_gyro_average)*(y_gyro_values[i]-y_gyro_average);
+  }
+  std_x=sqrt(std_x/BALANCE_GYRO_CAL_NUM_SAMPLES);
+  std_y=sqrt(std_y/BALANCE_GYRO_CAL_NUM_SAMPLES);
+  if(std_x<BALANCE_MAX_CAL_GYRO_ERROR && std_y<BALANCE_MAX_CAL_GYRO_ERROR)
+  {
+    balance_gyro_calibrated=0x01;
+    balance_x_gyro_center=(uint16_t)x_gyro_average;
+    balance_y_gyro_center=(uint16_t)y_gyro_average;
+  }
+  else
+  {
+    buzzer_start_alarm(BALANCE_GYRO_CAL_FAILED_ALARM_NOTE,BALANCE_GYRO_CAL_FAILED_ALARM_TIME_ON,BALANCE_GYRO_CAL_FAILED_ALARM_TIME_OFF);
+    return 0x00;
+  }
+  buzzer_stop_alarm();
+  return 0x01;
+}
+
+void balance_enable(void)
+{
+  balance_enabled=0x01;
+}
+
+void balance_disable(void)
+{
+  balance_enabled=0x00;
+}
+
+uint8_t balance_is_enabled(void)
+{
+  return balance_enabled;
+}
diff --git a/motion/src/examples/Makefile b/motion/src/examples/Makefile
index 63e7191ae722f8bba643d837c64e842202220a4c..6b50b49effa4336102f6b86be4556fea9f5fa6af 100644
--- a/motion/src/examples/Makefile
+++ b/motion/src/examples/Makefile
@@ -1,80 +1,55 @@
-ROBOTIS_C_DIR =../../../robotis_lib
+PROJECT=manager_ex
+########################################################
+# afegir tots els fitxers que s'han de compilar aquí
+########################################################
+SOURCES=main.c
 
-ifdef ROBOTIS_C_DIR
-    ROBOTIS_C_DIR_FOUND=true
-else
-    ROBOTIS_C_DIR_FOUND=false
-endif
-
-PROJECT=example
-SOURCES=main.c 
 OBJS=$(SOURCES:.c=.o)
-SDIR=../../src/ 
-IDIR=../../include/
-BDIR=../../build/
-LDIR=../../lib/
+SRC_DIR=./src/
+DEV_DIR=../../../dyn_devices/
+COMM_DIR=../../../communications/
+CONT_DIR=../../../controllers/
+MAN_DIR=../../
 CC=avr-gcc
-OBJCOPY=avr-ar rsc
+OBJCOPY=avr-objcopy
 MMCU=atmega2561
-CONTLIB=../../../controllers/lib/
-SERVLIB=../../../dyn_devices/lib/
-ROBOTISLIB=$(ROBOTIS_C_DIR)/lib/
 
-ROBOTIS_INCLUDE_DIR=$(ROBOTIS_C_DIR)/include/
+LIBS=$(CONT_DIR)lib/libcontrollers.a $(MAN_DIR)lib/libmotion_manager.a $(COMM_DIR)lib/libcomm.a $(DEV_DIR)lib/libdyn_devices.a
 
-SERVOS_INCLUDE_DIR=../../../dyn_devices/include/
+INCLUDE_DIRS=-I$(DEV_DIR)include -I$(COMM_DIR)include -I$(CONT_DIR)include -I$(MAN_DIR)include
 
-CONTROLLER_INCLUDE_DIR=../../../controllers/include/
+CFLAGS=-mmcu=$(MMCU) -Wall -Os $(defines) -DF_CPU=16000000UL -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
 
-CFLAGS=-mmcu=$(MMCU) -Wall -O3 -DF_CPU=16000000UL -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
+LDFLAGS=-mmcu=$(MMCU) -Wl,-Map=$(PROJECT).map -DF_CPU=16000000UL
 
-.PHONY: all check_robotis_c show_banner clean
+HEX_FLASH_FLAGS = -R .eeprom -R .fuse -R .lock -R .signature
 
-all: check_robotis_c show_banner $(PROJECT).elf $(PROJECT).hex
+.PHONY: all
 
-check_robotis_c:
-	@if ! $(ROBOTIS_C_DIR_FOUND); then                                                 \
-	    echo " IMPORTANT !!! ";                                                        \
-	    echo " ROBOTIS_C_DIR variable is not set! The software won't compile";         \
-	    echo " unless you specify the path to Robotis embedded C software.";           \
-	    echo "";                                                                       \
-	    echo " You should:";                                                           \
-	    echo "   1. Edit the makefile, at the begging set up ROBOTIS_C_DIR";           \
-	    echo "   2. Use the variable with make: make ROBOTIS_C_DIR=path-to-bioloid-c"; \
-	    echo "";                                                                       \
-	    echo " Beware of:";                                                            \
-	    echo "   1. Do not use '(' in the directory name (as it comes from robotis) "; \
-	    echo "   2. Absolute path is preferred to avoid problems with submake calls"; \
-	    exit 2;                                                                        \
-	fi
+all: communications dyn_devices controllers motion_manager $(PROJECT).hex
 
-	@if [ ! -d $(ROBOTIS_C_DIR) ]; then                                                  \
-	   echo " ! ROBOTIS_C_DIR is pointing to a non existant directory. Please fix it.";  \
-	   exit 2;                                                                           \
-	fi
+$(PROJECT).hex: $(PROJECT).elf
+	$(OBJCOPY) -O ihex $(HEX_FLASH_FLAGS)  $< $@
+$(PROJECT).elf: $(OBJS)
+	$(CC) $(LDFLAGS) $(OBJS) $(LIBS) -o $(PROJECT).elf
+%.o:%.c
+	$(CC) -c $(CFLAGS) $(INCLUDE_DIRS) -o $@ $<
 
-show_banner:
-	@echo "------------------------------------------------------";
-	@echo "       _____                                          ";
-	@echo "      /  _  \                                         ";
-	@echo "      | |_| |               The Humanoid Lab          ";
-	@echo "  ____\_____/____                                     ";
-	@echo " /               \    http://apollo.upc.es/humanoide/ ";
-	@echo "/   _         _   \                                   ";
-	@echo "|  | |       | |  |            $(PROJECT)             ";
-	@echo "|  | |       | |  |                                   ";
-	@echo "------------------------------------------------------";
+communications:
+	$(MAKE) -C $(COMM_DIR)
 
+dyn_devices:
+	$(MAKE) -C $(DEV_DIR)
 
-$(PROJECT).elf: ${OBJS}
-	$(CC) -g -mmcu=$(MMCU) -o ${LDIR}$(PROJECT).elf $(OBJS) -L $(LDIR) -l motion_manager -L $(CONTLIB) -l controllers -L $(SERVLIB) -l dyn_devices -L $(ROBOTISLIB) -l dynamixel -L $(ROBOTISLIB) -l serial
+controllers:
+	$(MAKE) -C $(CONT_DIR)
 
-$(PROJECT).hex: $(PROJECT).elf
-	avr-objcopy -O ihex $(LDIR)$(PROJECT).elf $(PROJECT).hex 
+motion_manager:
+	$(MAKE) -C $(MAN_DIR)
 
-%.o: %.c 
-	$(CC) -c $(CFLAGS) -I$(IDIR) -I${ROBOTIS_INCLUDE_DIR} -I${SERVOS_INCLUDE_DIR} -I${CONTROLLER_INCLUDE_DIR} -o $@ $<
+download: $(MAIN_OUT_HEX)
+	fw_downloader -d /dev/ttyUSB0 -f ./src/examples/comm_ex.hex -p new
 
 clean:
-	rm -f ${LDIR}$(PROJECT).a
-	rm -f $(OBJS) 
+	-rm $(PROJECT).*
+	-rm $(OBJS)
diff --git a/motion/src/examples/main.c b/motion/src/examples/main.c
index 3a50003e54f89fc451dccd8f42ae6746c1a085a7..eef8267f38155f31e8159cd195e9e5588f6f0118 100644
--- a/motion/src/examples/main.c
+++ b/motion/src/examples/main.c
@@ -1,65 +1,38 @@
+#include <util/delay.h>
+#include <stdio.h>
+#include "motion_cfg.h"
 #include "motion_manager.h"
 #include "action.h"
 #include "cm510.h"
-#include <util/delay.h>
-#include <stdio.h>
-#include "exp_board.h"
-#include "exp_board_reg.h"
-#include "serial.h"
-
+#include "gpio.h"
+#include "serial_console.h"
 
-int main(void)
+void user_init(void)
 {
-	int n_servos = 18;
-	init_cm510(ASYNC);
-
-  manager_init(n_servos);
-
-  int exp_board_ad_4, exp_board_ad_5, gyro_x, gyro_y;
-
-  //serial_initialize(57600);
-  exp_board_init(192);
-
-
-  action_init();
-	action_set_page(1);
-	action_start_page();
-	_delay_ms(2000);
+  serial_console_init(57600);
+}
 
-	if(manager_calibrate_gyro())
-	{
-		manager_enable_gyro();
-	  action_set_page(3);
-	  action_start_page();	
+void user_loop(void)
+{
+  if(is_button_falling_edge(BTN_UP))
+  {
+    if(is_action_running()==0x00)
+    {
+      printf("start action\n");
+      action_set_page(3);
+      action_start_page();
+    }
   }
-  
-  exp_board_ad_4 = get_adc_avg_channel(ADC4);
-  while(exp_board_ad_4 < 400)
+  else
   {
-  	exp_board_ad_4 = get_adc_avg_channel(ADC4);
-  	//printf("exp_board_ad_4: %d\n",exp_board_ad_4);		
-  	_delay_ms(50);
+    if(is_button_falling_edge(BTN_DOWN))
+    {
+      if(is_action_running())
+      {
+        printf("stop action\n");
+        action_stop_page();
+      }
+    }
   }
-  //printf("stop");	
-	action_stop_page();
-
-	//while(is_action_running())	
-	action_set_page(1);
-	action_start_page();
-
-	//_delay_ms(1000);
-	//action_set_page(32);
-	//action_start_page();
-	
-	while(1) {
-    //exp_board_ad_4 = get_adc_avg_channel(ADC4);
-    //printf("exp_board_ad_4: %d\n",exp_board_ad_4);		
-
-    //exp_board_ad_5 = get_adc_avg_channel(ADC5);
-    //printf("exp_board_ad_5: %d\n\n",exp_board_ad_5);		
-		//turn_led_on(LED_PROGRAM);
-		//action_process();
-		//turn_led_off(LED_PROGRAM);
-		//_delay_ms(500);
-	}
 }
+
diff --git a/motion/src/motion_manager.c b/motion/src/motion_manager.c
index 5c435f671797a38464a9d84730c9812b714a06e0..129c42640172f9cb1f5ee4b3f45df1d6bcf474c6 100644
--- a/motion/src/motion_manager.c
+++ b/motion/src/motion_manager.c
@@ -1,26 +1,30 @@
-#include "motion_manager.h"
 #include <avr/io.h>
 #include <avr/interrupt.h>
 #include <util/delay.h>
+#include "motion_cfg.h"
+#include "motion_manager.h"
+#include "motion_pages.h"
 #include "dynamixel_master.h"
 #include "dyn_servos.h"
 #include "dyn_servos_reg.h"
 #include "action.h"
+#include "balance.h"
 #include "buzzer.h"
+#include "gpio.h"
 
 // external functions
 extern void buzzer_start_alarm(note_t note,uint16_t on_time_100ms,uint16_t off_time_100ms);
 extern void buzzer_stop_alarm(void);
+extern void balance_loop(void);
+extern void balance_get_all_offsets(int16_t **offsets);
+extern void action_init(void);
+extern void action_process(void);
 
 // private variables
 uint8_t manager_num_servos;
 TServoInfo manager_servos[MANAGER_MAX_NUM_SERVOS];
-
-// gyroscope private variables
-uint16_t x_gyro_center;
-uint16_t y_gyro_center;
-uint8_t gyro_enabled;
-uint8_t robot_fallen_state;
+extern TPage action_next_page;
+extern TPage action_current_page;
 
 typedef struct
 {
@@ -34,18 +38,38 @@ dyn_send_data data[MANAGER_MAX_NUM_SERVOS];
 TWriteData packets[MANAGER_MAX_NUM_SERVOS];
 
 // private functions
-void manager_loop(void)
+void manager_timer_init(void)
 {
-//  turn_led_on(LED_PROGRAM); //debug
-
-  // call the action process
-  action_process(); //action_ready
-  // balance the robot
-  manager_balance();
-  // send the motion commands to the servos
-  manager_send_motion_command();
+  // initialize timer4 for buzzer time control (7.8ms period)
+  TIMSK4=0x00;// disable all interrupts
+  TIFR4=0x2F;// clear any pending interrupt
+  TCNT4H=0x00;                   
+  TCNT4L=0x00;
+  // set PWM mode with ICR top-count (CTC mode)
+  TCCR4A=0x00;
+  TCCR4A&=~(1<<WGM40);
+  TCCR4A&=~(1<<WGM41);
+  TCCR4B=0x00;
+  TCCR4B|=(1<<WGM42);
+  TCCR4B&=~(1<<WGM43);
+  // clear output compare value A
+  OCR4AH=0x3D;
+  OCR4AL=0x08;
+  TCCR4B&=0xF8;// set the prescaler to 8
+  TCCR4B|=0x02;
+}
 
-//  turn_led_off(LED_PROGRAM); //debug
+uint8_t manager_period_done(void)
+{
+  if(TIFR4&(1<<OCF4A))
+  {
+    TIFR4|=(1<<OCF4A);// clear any interrupt
+    TCNT4H=0x00;                   
+    TCNT4L=0x00;
+    return 0x01;
+  }
+  else
+    return 0x00;
 }
 
 void manager_send_motion_command(void) 
@@ -53,12 +77,14 @@ void manager_send_motion_command(void)
   uint8_t *pdata;
   uint8_t i;
   uint16_t target_pos;
+  int16_t *offsets;
 
-  ppacket = (uint8_t *)data;
+  pdata = (uint8_t *)data;
   
+  balance_get_all_offsets(&offsets);
   for(i=0;i<manager_num_servos;i++) 
   {
-    target_pos = manager_servos[i].current_value + servo_offsets[i];
+    target_pos = manager_servos[i].current_value + offsets[i];
     pdata[i*4+2] = target_pos&0xFF;
     pdata[i*4+3] = (target_pos>>8)&0xFF;
 
@@ -72,42 +98,10 @@ void manager_send_motion_command(void)
     else 
       pdata[i*4+1] = 1<<(0x0F&manager_servos[i].ccw_slope);
 
-    packets[i].data=(uint8_t *)&data[i];
-  }
-  dyn_master_sync_write(manager_num_servos,servo_ids,CW_COMP_SLOPE,4,packets); 
-}
-
-void manager_balance(void)
-{
-  int16_t gyro_x=0;
-  int16_t gyro_y=0;
-
-  if(gyro_enabled == 0x01)
-  {
-    // get x,y gyroscope
-//    gyro_x=get_adc_channel(X_GYRO_CHANNEL)-x_gyro_center;
-//    gyro_y=get_adc_channel(Y_GYRO_CHANNEL)-y_gyro_center;
-
-    if(gyro_x > 200)
-      robot_fallen_state = 0x02;
-    else if(gyro_x < -200)
-      robot_fallen_state = 0x01;
-
-    float x_error1,x_error2,y_error1,y_error2;
-    x_error1=gyro_x*4.0/54.0;
-    x_error2=gyro_x*4.0/18.0;
-    y_error1=gyro_y*4.0/20.0;
-    y_error2=gyro_y*4.0/40.0;
-
-    servo_offsets[8] = (uint16_t)y_error1;
-    servo_offsets[9] = (uint16_t)y_error1;
-    servo_offsets[12] = (uint16_t)x_error1;//-1.0;
-    servo_offsets[13] = (uint16_t)-x_error1;//+1.0;
-    servo_offsets[14] = (uint16_t)+x_error2;
-    servo_offsets[15] = (uint16_t)-x_error2;
-    servo_offsets[16] = (uint16_t)-y_error2;
-    servo_offsets[17] = (uint16_t)-y_error2;
+    packets[i].data_addr=(uint8_t *)&data[i];
   }
+  if(manager_num_servos>0)
+    dyn_master_sync_write(manager_num_servos,servo_ids,CW_COMP_SLOPE,4,packets); 
 }
 
 void manager_get_current_position(void)
@@ -133,6 +127,11 @@ void manager_set_servo_slopes(uint8_t servo_id, uint8_t slope)
   }
 }
 
+void manager_set_index_value(uint8_t servo_id, uint16_t value)
+{
+  manager_servos[servo_id].current_value=value;
+}
+
 void manager_set_servo_value(uint8_t servo_id, uint16_t value)
 {
   uint8_t i;
@@ -147,11 +146,6 @@ void manager_set_servo_value(uint8_t servo_id, uint16_t value)
   }
 }
 
-void manager_set_index_value(uint8_t servo_id, uint16_t value)
-{
-  manager_servos[servo_id].current_value=value;
-}
-
 uint16_t manager_get_index_value(uint8_t servo_id)
 {
   return manager_servos[servo_id-1].current_value;
@@ -168,24 +162,23 @@ uint16_t manager_get_servo_value(uint8_t servo_id)
   return 2048;
 }
 
-void manager_timer_init(void)
-{
-
-}
-
-uint8_t manager_period_done(void)
+void manager_loop(void)
 {
-
+  if(manager_period_done()==0x01)
+  {
+    // call the action process
+    action_process(); //action_ready
+    // balance the robot
+    balance_loop();
+    // send the motion commands to the servos
+    manager_send_motion_command();
+  }
 }
 
 // public functions
-void manager_init(uint8_t num_servos)
+uint8_t manager_init(uint8_t num_servos)
 {
   uint8_t i;
-  uint8_t found_servos;
-
-  /* initialize private variables */
-  robot_fallen_state = 0x00;
 
   // enable power to the servos
   dyn_master_init();
@@ -194,13 +187,14 @@ void manager_init(uint8_t num_servos)
   /* scan the bus for all available servos */
   dyn_master_scan(&manager_num_servos,servo_ids);
   if(num_servos != manager_num_servos)
-    buzzer_start_alarm(NOTE_SOL,1000,10000);
+    buzzer_start_alarm(MANAGER_MISSING_SERVOS_ALARM_NOTE,MANAGER_MISSING_SERVOS_ALARM_TIME_ON,MANAGER_MISSING_SERVOS_ALARM_TIME_OFF);
+  else
+    buzzer_stop_alarm();
 
   for(i=0;i<manager_num_servos;i++)
   {
     //set_target_speed(i+1,0);
-    servo_offsets[i] = 0;
-    enable_servo(i+1);
+    enable_servo(servo_ids[i]);
     manager_servos[i].id=servo_ids[i];
     manager_servos[i].max_value=1023;
     manager_servos[i].min_value=0;
@@ -210,74 +204,11 @@ void manager_init(uint8_t num_servos)
 
   /* initialize the period timer */
   manager_timer_init();
-}
-
-uint8_t manager_check_robot_fallen(void)
-{
-  uint8_t state = robot_fallen_state;
-  robot_fallen_state = 0x00;
-  return state;
-}
-
-uint8_t manager_calibrate_gyro(void)
-{
-  uint16_t x_gyro_values[GYRO_CAL_NUM_SAMPLES];
-  uint16_t y_gyro_values[GYRO_CAL_NUM_SAMPLES];
-  float x_gyro_average=0.0,y_gyro_average=0.0,std_x=0.0,std_y=0.0;
-  uint8_t calibrated=0x00;
-  uint8_t i=0,count=0;
+  /* initialize the action module */
+  action_init();
 
-  x_gyro_center=0;
-  y_gyro_center=0;
-  while(!calibrated)
-  {
-    for(i=0;i<GYRO_CAL_NUM_SAMPLES;i++)
-    {
-      _delay_ms(50);
-//      x_gyro_values[i]=get_adc_channel(X_GYRO_CHANNEL); 
-//      y_gyro_values[i]=get_adc_channel(Y_GYRO_CHANNEL); 
-      x_gyro_average+=x_gyro_values[i];
-      y_gyro_average+=y_gyro_values[i];
-    }  
-    x_gyro_average/=(float)GYRO_CAL_NUM_SAMPLES;
-    y_gyro_average/=(float)GYRO_CAL_NUM_SAMPLES;
-    // compute the standard deviation
-    for(i=0;i<GYRO_CAL_NUM_SAMPLES;i++)
-    {
-      std_x+=(x_gyro_values[i]-x_gyro_average)*(x_gyro_values[i]-x_gyro_average);
-      std_y+=(y_gyro_values[i]-y_gyro_average)*(y_gyro_values[i]-y_gyro_average);
-    }
-    std_x=sqrt(std_x/GYRO_CAL_NUM_SAMPLES);
-    std_y=sqrt(std_y/GYRO_CAL_NUM_SAMPLES);
-    if(std_x<20.0 && std_y<20.0)
-    {
-      calibrated=0x01;
-      x_gyro_center=(uint16_t)x_gyro_average;
-      y_gyro_center=(uint16_t)y_gyro_average;
-    }
-    else
-    {
-      count++;
-      if(count==5)
-        return 0x00;
-    }
-  }
   return 0x01;
 }
 
-void manager_enable_gyro(void)
-{
-  gyro_enabled=0x01;
-}
-
-void manager_disable_gyro(void)
-{
-  gyro_enabled=0x00;
-}
-
-uint8_t manager_is_gyro_enabled(void)
-{
-  return gyro_enabled;
-}
 
 
diff --git a/motion/src/motion_pages.c b/motion/src/motion_pages.c
index c8641f9e46af6e56dca137118ca738206b6f0e7c..3b53c1e3538c87ef5189423388d693b7d7f81263 100644
--- a/motion/src/motion_pages.c
+++ b/motion/src/motion_pages.c
@@ -1,14 +1,14 @@
 #include "motion_pages.h"
-#include "cm510.h"
 
-void pages_get_page(uint8_t page_id,TPage *page) {
-	uint8_t i=0;
-	uint8_t *ppage = (uint8_t *)page;
-	TPage *base_page = &((TPage *)PAGE_BASE_ADDR)[page_id];
-  
+void pages_get_page(uint8_t page_id,TPage *page) 
+{
+  uint8_t i=0;
+  uint8_t *ppage = (uint8_t *)page;
+  TPage *base_page = &((TPage *)PAGE_BASE_ADDR)[page_id];
+
   for(i=0;i<sizeof(TPage);i++) {
-		ppage[i]= pgm_read_byte_far((uint8_t *)base_page+i);
-	}
+    ppage[i]= pgm_read_byte_far((uint8_t *)base_page+i);
+  }
 }
 
 uint8_t pages_check_checksum(TPage *page)
@@ -40,24 +40,18 @@ void pages_copy_page(TPage *src,TPage *dst)
     ((uint8_t *)dst)[i]=((volatile uint8_t *)src)[i];
 }
 
-
-uint8_t load_page_info(uint8_t page_id,TPage *page) {
-//	uint8_t *start_address;
-//	uint16_t i;
-
-//	start_address=(uint8_t *)POSE_BASE_ADR+(0x200*page_id);
-
-//	for(i=0;i<0x200;i++) (((uint8_t *)page)[i])=pgm_read_byte(start_address+i);
+uint8_t load_page_info(uint8_t page_id,TPage *page) 
+{
   uint32_t i=0;
   uint8_t *ppage = (uint8_t *)page;
-//  TPage *base_page = (TPage *)PAGE_BASE_ADDR;
 
-  for(i=0;i<sizeof(TPage);i++) {
+  for(i=0;i<sizeof(TPage);i++) 
     ppage[i]= pgm_read_byte_far((uint32_t)PAGE_BASE_ADDR+(uint32_t)(sizeof(TPage)*page_id)+i);
-//    ppage[i]= pgm_read_byte_far( &(base_page[page_id])+i);
-  }
-	if(pages_check_checksum(page)==0) return 0;
-	else return -1;
+  
+  if(pages_check_checksum(page)==0) 
+    return 0;
+  else 
+    return -1;
 }