diff --git a/communications/include/dynamixel.h b/communications/include/dynamixel.h
index 40736bd92d89d9ce75a9ecaf9376a34c8ac3931b..dd7560bf08e04ab74cd32daca41426a1d47132a9 100644
--- a/communications/include/dynamixel.h
+++ b/communications/include/dynamixel.h
@@ -1,6 +1,10 @@
 #ifndef _DYNAMIXEL_H
 #define _DYNAMIXEL_H
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+
 #include "dynamixel_common.h"
 #include <avr/io.h>
 #include <avr/interrupt.h>
@@ -13,18 +17,9 @@
 #define     DYN_DATA_OFF        5
 
 void dyn_copy_packet(uint8_t *source, uint8_t *destination);
-inline uint8_t dyn_get_id(uint8_t *packet)
-{
-  return packet[DYN_ID_OFF];
-}
-inline uint8_t dyn_get_length(uint8_t *packet)
-{
-  return packet[DYN_LENGTH_OFF];
-}
-inline TDynInstruction dyn_get_instruction(uint8_t *packet)
-{
-  return packet[DYN_INST_OFF];
-}
+uint8_t dyn_get_id(uint8_t *packet);
+uint8_t dyn_get_length(uint8_t *packet);
+TDynInstruction dyn_get_instruction(uint8_t *packet);
 uint8_t dyn_check_checksum(uint8_t *packet);
 
 // instruction packet
@@ -42,35 +37,17 @@ typedef struct{
 void dyn_init_ping_packet(uint8_t *packet,uint8_t id);
 /* read instruction */
 void dyn_init_read_packet(uint8_t *packet,uint8_t id,uint8_t address,uint8_t length);
-inline uint8_t dyn_get_read_length(uint8_t *packet)
-{
-  return packet[DYN_DATA_OFF+1];
-}
-inline uint8_t dyn_get_read_address(uint8_t *packet)
-{
-  return packet[DYN_DATA_OFF];
-}
+uint8_t dyn_get_read_length(uint8_t *packet);
+uint8_t dyn_get_read_address(uint8_t *packet);
 /* write instruction */
 void dyn_init_write_packet(uint8_t *packet,uint8_t id,uint8_t address,uint8_t length,uint8_t *data);
-inline uint8_t dyn_get_write_address(uint8_t *packet)
-{
-  return packet[DYN_DATA_OFF];
-}
-inline uint8_t dyn_get_write_length(uint8_t *packet)
-{
-  return packet[DYN_LENGTH_OFF]-3;
-}
+uint8_t dyn_get_write_address(uint8_t *packet);
+uint8_t dyn_get_write_length(uint8_t *packet);
 uint8_t dyn_get_write_data(uint8_t *packet,uint8_t *data);
 /* registered write instruction */
 void dyn_init_reg_write_packet(uint8_t *packet,uint8_t id,uint8_t address,uint8_t length,uint8_t *data);
-inline uint8_t dyn_get_reg_write_address(uint8_t *packet)
-{
-  return packet[DYN_DATA_OFF];
-}
-inline uint8_t dyn_get_reg_write_length(uint8_t *packet)
-{
-  return packet[DYN_LENGTH_OFF]-3;
-}
+uint8_t dyn_get_reg_write_address(uint8_t *packet);
+uint8_t dyn_get_reg_write_length(uint8_t *packet);
 uint8_t dyn_get_reg_write_data(uint8_t *packet,uint8_t *data);
 /* action instruction */
 void dyn_init_action_packet(uint8_t *packet);
@@ -96,11 +73,12 @@ typedef struct{
 */
 
 void dyn_init_status_packet(uint8_t *packet,uint8_t id,TDynError error,uint8_t length,uint8_t *data);
-inline TDynError dyn_get_status_error(uint8_t *packet)
-{
-  return packet[DYN_ERROR_OFF];
-}
+TDynError dyn_get_status_error(uint8_t *packet);
 /* read instruction status packet */
 uint8_t dyn_get_read_status_data(uint8_t *packet,uint8_t *data);
 
+#ifdef __cplusplus
+}
+#endif
+
 #endif
diff --git a/communications/include/dynamixel_common.h b/communications/include/dynamixel_common.h
index ea125c550f3593b27a54b4f6469a947ac5f14446..d27bfdf9591c3c9667ebf8387ffe21ff58f3ec03 100644
--- a/communications/include/dynamixel_common.h
+++ b/communications/include/dynamixel_common.h
@@ -1,6 +1,10 @@
 #ifndef _DYNAMIXEL_COMMON_H
 #define _DYNAMIXEL_COMMON_H
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+
 #define     MAX_DATA_LENGTH     255
 
 #define     MAX_HEADER_LENGTH   6
@@ -46,4 +50,9 @@ typedef enum{DYN_NO_ERROR=0x00,
              DYN_VOLTAGE_ERROR=0x01} TDynError;
 
 typedef enum{DYN_VER1=0x01,DYN_VER2=0x02} TDynVersion;
+
+#ifdef __cplusplus
+}
+#endif
+
 #endif
diff --git a/communications/include/dynamixel_master.h b/communications/include/dynamixel_master.h
index 9fa62737f3c26e3e75d2bfbc888cc93d2524dd66..c1f76dfcd2a5e550c5a2d2a476dd22a8d59dce74 100644
--- a/communications/include/dynamixel_master.h
+++ b/communications/include/dynamixel_master.h
@@ -1,6 +1,10 @@
 #ifndef _DYNAMIXEL_MASTER_H
 #define _DYNAMIXEL_MASTER_H
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+
 #include "dynamixel.h"
 
 /* public functions */
@@ -23,4 +27,8 @@ uint8_t dyn_master_action(void);
 uint8_t dyn_master_sync_write(uint8_t num,uint8_t *ids,uint16_t address, uint16_t length, TWriteData *data);
 uint8_t dyn_master_bulk_read(uint8_t num,uint8_t *ids,uint16_t *address, uint16_t *length, TWriteData *data);
 
+#ifdef __cplusplus
+}
+#endif
+
 #endif
diff --git a/communications/include/serial_console.h b/communications/include/serial_console.h
index 2624cfa550762791faee2eec945068763002ddb4..c2c1d0f98afe511c8367d8a7f0de89b63837cf3f 100644
--- a/communications/include/serial_console.h
+++ b/communications/include/serial_console.h
@@ -1,9 +1,17 @@
 #ifndef _SERIAL_CONSOLE_H
 #define _SERIAL_CONSOLE_H
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+
 #include <avr/io.h>
 #include <avr/interrupt.h>
 
 void serial_console_init(uint32_t baudrate);
 
+#ifdef __cplusplus
+}
+#endif
+
 #endif
diff --git a/communications/src/dynamixel.c b/communications/src/dynamixel.c
index cdff1dd42c5842829bfd1b1528bd6a85d620fcc9..cc9307d4800555655ad04b99917e736616534c57 100644
--- a/communications/src/dynamixel.c
+++ b/communications/src/dynamixel.c
@@ -1,6 +1,6 @@
 #include "dynamixel.h"
 
-// provate functions
+// private functions
 void dyn_set_checksum(uint8_t *packet)
 {
   uint8_t checksum=0x00;
@@ -39,6 +39,20 @@ void dyn_copy_packet(uint8_t *source, uint8_t *destination)
     destination[i]=source[i];
 }
 
+uint8_t dyn_get_id(uint8_t *packet)
+{
+  return packet[DYN_ID_OFF];
+}
+
+uint8_t dyn_get_length(uint8_t *packet)
+{
+  return packet[DYN_LENGTH_OFF];
+}
+
+TDynInstruction dyn_get_instruction(uint8_t *packet)
+{
+  return packet[DYN_INST_OFF];
+}
 /* ping instruction*/
 void dyn_init_ping_packet(uint8_t *packet,uint8_t id)
 {
@@ -49,7 +63,6 @@ void dyn_init_ping_packet(uint8_t *packet,uint8_t id)
   packet[DYN_INST_OFF]=DYN_PING;
   dyn_set_checksum(packet);
 }
-
 /* read instruction */
 void dyn_init_read_packet(uint8_t *packet,uint8_t id,uint8_t address,uint8_t length)
 {
@@ -63,6 +76,16 @@ void dyn_init_read_packet(uint8_t *packet,uint8_t id,uint8_t address,uint8_t len
   dyn_set_checksum(packet);
 }
 
+uint8_t dyn_get_read_length(uint8_t *packet)
+{
+  return packet[DYN_DATA_OFF+1];
+}
+
+uint8_t dyn_get_read_address(uint8_t *packet)
+{
+  return packet[DYN_DATA_OFF];
+}
+
 /* write instruction */
 void dyn_init_write_packet(uint8_t *packet,uint8_t id,uint8_t address,uint8_t length,uint8_t *data)
 {
@@ -79,6 +102,16 @@ void dyn_init_write_packet(uint8_t *packet,uint8_t id,uint8_t address,uint8_t le
   dyn_set_checksum(packet);
 }
 
+uint8_t dyn_get_write_address(uint8_t *packet)
+{
+  return packet[DYN_DATA_OFF];
+}
+
+uint8_t dyn_get_write_length(uint8_t *packet)
+{
+  return packet[DYN_LENGTH_OFF]-3;
+}
+
 uint8_t dyn_get_write_data(uint8_t *packet,uint8_t *data)
 {
   uint8_t i;
@@ -105,6 +138,16 @@ void dyn_init_reg_write_packet(uint8_t *packet,uint8_t id,uint8_t address,uint8_
   dyn_set_checksum(packet);
 }
 
+uint8_t dyn_get_reg_write_address(uint8_t *packet)
+{
+  return packet[DYN_DATA_OFF];
+}
+
+uint8_t dyn_get_reg_write_length(uint8_t *packet)
+{
+  return packet[DYN_LENGTH_OFF]-3;
+}
+
 uint8_t dyn_get_reg_write_data(uint8_t *packet,uint8_t *data)
 {
   uint8_t i;
@@ -229,6 +272,11 @@ void dyn_init_status_packet(uint8_t *packet,uint8_t id,TDynError error,uint8_t l
   dyn_set_checksum(packet); 
 }
 
+TDynError dyn_get_status_error(uint8_t *packet)
+{
+  return packet[DYN_ERROR_OFF];
+}
+
 /* read instruction status packet */
 uint8_t dyn_get_read_status_data(uint8_t *packet,uint8_t *data)
 {
diff --git a/controllers/include/adc.h b/controllers/include/adc.h
index a5b45094cf4da6a4d3b5f3280aeef76c3c175a63..132f62f0e02252e2b89b6f47cf177bc7eb3092e0 100644
--- a/controllers/include/adc.h
+++ b/controllers/include/adc.h
@@ -1,6 +1,10 @@
 #ifndef _ADC_H
 #define _ADC_H
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+
 #include <avr/io.h>
 #include <avr/interrupt.h>
 
@@ -47,4 +51,8 @@ void init_adc(void);
 uint16_t get_adc_channel(adc_t channel_id);
 uint16_t get_adc_avg_channel(adc_t channel_id);
 
+#ifdef __cplusplus
+}
+#endif
+
 #endif
diff --git a/controllers/include/buzzer.h b/controllers/include/buzzer.h
index 93c96b8ac81afee2d095a0ca58ff3727bcb06c92..92a172b325108095e8fb65c288e4700b53305040 100644
--- a/controllers/include/buzzer.h
+++ b/controllers/include/buzzer.h
@@ -1,6 +1,10 @@
 #ifndef _BUZZER_H
 #define _BUZZER_H
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+
 #include <avr/io.h>
 #include <avr/interrupt.h>
 
@@ -60,4 +64,8 @@ void buzzer_stop(void);
 uint8_t buzzer_is_playing(void);
 uint8_t buzzer_is_playing_alarm(void);
 
+#ifdef __cplusplus
+}
+#endif
+
 #endif
diff --git a/controllers/include/cm510.h b/controllers/include/cm510.h
index 8fee1b362e0ce6b1a6c33b0dde5d4a02b5ecb1dc..c4071376f552080b91dcc553227c7ed220051fd5 100755
--- a/controllers/include/cm510.h
+++ b/controllers/include/cm510.h
@@ -18,6 +18,10 @@
 #ifndef _CM510_H
 #define _CM510_H
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+
 #include "cont_cfg.h"
 #include "motion_cfg.h"
 #include "comm_cfg.h"
@@ -51,4 +55,8 @@
  */
 void init_cm510(void);
 
+#ifdef __cplusplus
+}
+#endif
+
 #endif
diff --git a/controllers/include/gpio.h b/controllers/include/gpio.h
index 994899170dd675a33a5552c74a97f4320801ed4e..8f217548505bfc9101b71eb9801f1454a89afe2a 100644
--- a/controllers/include/gpio.h
+++ b/controllers/include/gpio.h
@@ -1,6 +1,10 @@
 #ifndef _GPIO_H
 #define _GPIO_H
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+
 #include <avr/io.h>
 #include <avr/interrupt.h>
 
@@ -98,4 +102,8 @@ uint8_t is_button_rising_edge(pushbuttons_t button_id);
 
 uint8_t is_button_falling_edge(pushbuttons_t button_id);
 
+#ifdef __cplusplus
+}
+#endif
+
 #endif
diff --git a/controllers/include/user_time.h b/controllers/include/user_time.h
index ab080482202830d0982cb083051b1ee26e623483..e0fd74d01ff81a823e72235dd4762870ced31095 100644
--- a/controllers/include/user_time.h
+++ b/controllers/include/user_time.h
@@ -1,6 +1,10 @@
 #ifndef _USER_TIME_H
 #define _USER_TIME_H
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+
 #include <avr/io.h>
 #include <avr/interrupt.h>
 
@@ -12,4 +16,8 @@ uint8_t user_time_is_period_done(void);
 uint8_t user_time_is_done(void);
 void user_time_stop(void);
 
+#ifdef __cplusplus
+}
+#endif
+
 #endif
diff --git a/dyn_devices/include/dyn_common.h b/dyn_devices/include/dyn_common.h
index d11eed1f0955360f01840f2e0164d30a9ba9f2e7..55b7abbd138ee6bb3c08e654c5d6615ff8da1306 100644
--- a/dyn_devices/include/dyn_common.h
+++ b/dyn_devices/include/dyn_common.h
@@ -1,6 +1,10 @@
 #ifndef _DYN_COMMON_H
 #define _DYN_COMMON_H
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+
 #include <avr/io.h>
 
 #define            MODEL_NUM           ((unsigned char)0x00)
@@ -124,4 +128,8 @@ uint8_t get_baudrate(uint8_t servo_id);
  */
 uint8_t set_baudrate(uint8_t servo_id, uint8_t baudrate);
 
+#ifdef __cplusplus
+}
+#endif
+
 #endif
diff --git a/dyn_devices/include/dyn_servos.h b/dyn_devices/include/dyn_servos.h
index 7f34451ef17e3802e89fbfdb182e50be7143c3e9..0ae31d57e0000cbe8176fd99cb0c080d3b9da6ee 100755
--- a/dyn_devices/include/dyn_servos.h
+++ b/dyn_devices/include/dyn_servos.h
@@ -21,6 +21,10 @@
 #include <avr/io.h>
 #include "dyn_common.h"
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+
 /**
  * \brief Function to get the current angle limits of a given servo.
  * 
@@ -811,4 +815,8 @@ uint16_t get_punch(uint8_t servo_id);
  */
 uint8_t set_punch(uint8_t servo_id, uint16_t punch);
 
+#ifdef __cplusplus
+}
+#endif
+
 #endif
diff --git a/dyn_devices/include/exp_board.h b/dyn_devices/include/exp_board.h
index 67842c8182a6705d85b33354398fb06bce088d75..194b698b6e43e7cd33571c147fb5801d60724904 100755
--- a/dyn_devices/include/exp_board.h
+++ b/dyn_devices/include/exp_board.h
@@ -18,6 +18,10 @@
 #ifndef _EXP_BOARD_H
 #define _EXP_BOARD_H
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+
 #include <avr/io.h>
 #include "dyn_common.h"
 
@@ -108,4 +112,8 @@ uint8_t uart_usb_send_vector(uint8_t *data, uint16_t length);
 uint8_t uart_usb_is_data_available(void);
 uint8_t uart_usb_receive_data(void);
 
+#ifdef __cplusplus
+}
+#endif
+
 #endif
diff --git a/motion/include/action.h b/motion/include/action.h
index 929ad3fa569e85a042af698bd77a7e1e168f24ec..4b52262b62d9f198e182b0a2b674c82b16b729f1 100644
--- a/motion/include/action.h
+++ b/motion/include/action.h
@@ -1,10 +1,18 @@
 #ifndef _ACTION_H
 #define _ACTION_H
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+
 // basic motion interface
 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);
 
+#ifdef __cplusplus
+}
+#endif
+
 #endif
diff --git a/motion/include/balance.h b/motion/include/balance.h
index 5c227eb00a2be42b38b8fcdd38541ed20935aced..05a44eb8c8e665d19c3b814a6c684d6b8390edd3 100644
--- a/motion/include/balance.h
+++ b/motion/include/balance.h
@@ -1,6 +1,10 @@
 #ifndef _BALANCE_H
 #define _BALANCE_H
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+
 typedef enum {robot_standing,robot_face_down,robot_face_up} fallen_t;
 
 // public functions
@@ -47,4 +51,8 @@ void balance_disable_gyro(void);
  */
 uint8_t balance_is_gyro_enabled(void);
 
+#ifdef __cplusplus
+}
+#endif
+
 #endif
diff --git a/motion/include/motion_manager.h b/motion/include/motion_manager.h
index 29b0ee61b9c8905828b823f6c75135ba69be8682..3a61d63312d0bbb7b252442d7897c74ef981ed19 100644
--- a/motion/include/motion_manager.h
+++ b/motion/include/motion_manager.h
@@ -1,6 +1,10 @@
 #ifndef _MOTION_MANAGER_H
 #define _MOTION_MANAGER_H
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+
 #include <inttypes.h>
 
 // servo information structure
@@ -17,4 +21,8 @@ typedef struct{
 // public functions
 uint8_t manager_init(uint8_t num_servos);
 
+#ifdef __cplusplus
+}
+#endif
+
 #endif
diff --git a/motion/include/motion_pages.h b/motion/include/motion_pages.h
index ba90c41dc85ed8426f8ee2410bd568332d1f7d57..477a2839ec4d629bd632fe916744bde01d88c297 100644
--- a/motion/include/motion_pages.h
+++ b/motion/include/motion_pages.h
@@ -1,8 +1,11 @@
 #ifndef _MOTION_PAGES_H
 #define _MOTION_PAGES_H
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+
 #include <inttypes.h>
-#include <avr/pgmspace.h>
 
 #define PAGE_BASE_ADDR               (0x1E000UL)
 
@@ -28,27 +31,30 @@ typedef struct // Header Structure (total 64unsigned char)
   uint8_t checksum;         // checksum         31
   uint8_t slope[31];        // CW/CCW compliance slope  32~62
   uint8_t reserved6;        // reserved6        63
-} TPageHeader;
+} TActionPageHeader;
 
 typedef struct // Step Structure (total 64unsigned char)
 {
   int16_t position[PAGE_MAX_NUM_SERVOS];    // Joint position   0~61
   uint8_t pause;            // Pause time       62
   uint8_t time;             // Time             63
-} TStep;
+} TActionStep;
 
 typedef struct // Page Structure (total 512unsigned char)
 {
-  TPageHeader header;       // Page header  0~64
-  TStep steps[POSE_NUMBER_OF_POSES_PER_PAGE];           // Page step    65~511
-} TPage;
+  TActionPageHeader header;       // Page header  0~64
+  TActionStep steps[POSE_NUMBER_OF_POSES_PER_PAGE];           // Page step    65~511
+} TActionPage;
 
 // public functions
-void pages_get_page(uint8_t page_id,TPage *page);
-uint8_t pages_check_checksum(TPage *page);
-void pages_clear_page(TPage *page);
-void pages_copy_page(TPage *src,TPage *dst);
+uint8_t pages_check_checksum(TActionPage *page);
+void pages_clear_page(TActionPage *page);
+void pages_copy_page(TActionPage *src,TActionPage *dst);
 
-uint8_t load_page_info(uint8_t page_id,TPage *page);
+uint8_t load_page_info(uint8_t page_id,TActionPage *page);
+
+#ifdef __cplusplus
+}
+#endif
 
 #endif
diff --git a/motion/src/action.c b/motion/src/action.c
index a1bb962d44cbb34cb47673063687104788ab754a..a15ea0cd157095d20427202f07d643a2b1b5d4bc 100644
--- a/motion/src/action.c
+++ b/motion/src/action.c
@@ -36,8 +36,8 @@ uint8_t action_next_index;
 uint8_t action_current_index;
 uint8_t action_step_count;
 
-TPage action_next_page;
-TPage action_current_page;
+TActionPage action_next_page;
+TActionPage action_current_page;
 
 // motion variables
 uint16_t wpStartAngle1024[PAGE_MAX_NUM_SERVOS]; 
@@ -154,7 +154,6 @@ void action_process(void)
     {
       // MAIN Section Áغñ
       bSection = MAIN_SECTION;
-//      turn_led_off(LED_TxD);
       wUnitTimeNum =  wUnitTimeTotalNum - (wAccelStep << 1);
       for(bID=1;bID<=manager_num_servos;bID++)
       {
@@ -173,7 +172,6 @@ void action_process(void)
     {
       // POST Section Áغñ
       bSection = POST_SECTION;
-//      turn_led_off(LED_TxD);
       wUnitTimeNum = wAccelStep;
 
       for(bID=1;bID<=manager_num_servos;bID++)
@@ -187,20 +185,17 @@ void action_process(void)
       if( wPauseTime )
       {
         bSection = PAUSE_SECTION;
-//        turn_led_off(LED_TxD);
         wUnitTimeNum = wPauseTime;
       }
       else
       {
         bSection = PRE_SECTION;
-//        turn_led_on(LED_TxD);
       }
     }
     else if( bSection == PAUSE_SECTION )
     {
       // PRE Section Áغñ
       bSection = PRE_SECTION;
-//      turn_led_on(LED_TxD);
 
       for(bID=1;bID<=manager_num_servos;bID++)
       {
@@ -385,7 +380,6 @@ void action_process(void)
     wUnitTimeCount++;
     if( bSection == PAUSE_SECTION )
     {
-//      toggle_led(LED_AUX);
     }
     else
     {
diff --git a/motion/src/balance.c b/motion/src/balance.c
index d7461ef0fca2bfb56269050ca5a558a5045ec857..dcc984799cd8319eed0318c32ef299b0cf62cc57 100644
--- a/motion/src/balance.c
+++ b/motion/src/balance.c
@@ -1,6 +1,7 @@
 #include <avr/io.h>
 #include <avr/interrupt.h>
 #include <util/delay.h>
+#include <math.h>
 #include "motion_cfg.h"
 #include "balance.h"
 #include "adc.h"
@@ -76,7 +77,7 @@ void balance_init(void)
     balance_offsets[i]=0;
 }
 
-uint8_t balance_robot_has_fallen(void)
+fallen_t balance_robot_has_fallen(void)
 {
   uint8_t state = balance_robot_fallen_state;
   balance_robot_fallen_state = robot_standing;
diff --git a/motion/src/motion_manager.c b/motion/src/motion_manager.c
index 0737c700d40d85abd130e145de9fcc4065835687..2c6130e96aefe9f14784ec4a73ef9eaff814c3c8 100644
--- a/motion/src/motion_manager.c
+++ b/motion/src/motion_manager.c
@@ -23,8 +23,8 @@ extern void action_process(void);
 // private variables
 uint8_t manager_num_servos;
 TServoInfo manager_servos[MANAGER_MAX_NUM_SERVOS];
-extern TPage action_next_page;
-extern TPage action_current_page;
+extern TActionPage action_next_page;
+extern TActionPage action_current_page;
 
 typedef struct
 {
@@ -169,7 +169,7 @@ void manager_loop(void)
     // call the action process
     action_process(); //action_ready
     // balance the robot
-    balance_loop();
+//    balance_loop();
     // send the motion commands to the servos
     manager_send_motion_command();
   }
@@ -188,8 +188,7 @@ uint8_t manager_init(uint8_t num_servos)
 
   /* scan the bus for all available servos */
   dyn_master_scan(&num,servos);
- 
-  printf("Total devices: %d\n",num); 
+
   manager_num_servos=0;
   for(i=0;i<num;i++)
   {
@@ -207,12 +206,11 @@ uint8_t manager_init(uint8_t num_servos)
       manager_num_servos++;
     }
   } 
-  printf("Total servos: %d\n",manager_num_servos); 
 
-  if(num_servos != manager_num_servos)
+/*  if(num_servos != manager_num_servos)
     buzzer_start_alarm(MANAGER_MISSING_SERVOS_ALARM_NOTE,MANAGER_MISSING_SERVOS_ALARM_TIME_ON,MANAGER_MISSING_SERVOS_ALARM_TIME_OFF);
   else
-    buzzer_stop_alarm();
+    buzzer_stop_alarm();*/
 
   /* initialize the period timer */
   manager_timer_init();
diff --git a/motion/src/motion_pages.c b/motion/src/motion_pages.c
index ef28d68ea22f1f88f867559c985f1ed40e310420..262e453b9fc7d20d311a467eab33713d2902b8a5 100644
--- a/motion/src/motion_pages.c
+++ b/motion/src/motion_pages.c
@@ -1,22 +1,12 @@
 #include "motion_pages.h"
+#include <avr/pgmspace.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];
-
-  for(i=0;i<sizeof(TPage);i++) {
-    ppage[i]= pgm_read_byte_far((uint8_t *)base_page+i);
-  }
-}
-
-uint8_t pages_check_checksum(TPage *page)
+uint8_t pages_check_checksum(TActionPage *page)
 {
   uint8_t checksum=0x00;
   uint16_t i=0;
 
-  for(i=0;i<sizeof(TPage);i++)
+  for(i=0;i<sizeof(TActionPage);i++)
     checksum+=((uint8_t *)page)[i];
   if(checksum==0x00)
     return 0x01;
@@ -24,29 +14,29 @@ uint8_t pages_check_checksum(TPage *page)
     return 0x00;
 }
 
-void pages_clear_page(TPage *page)
+void pages_clear_page(TActionPage *page)
 {
   uint16_t i=0;
 
-  for(i=0;i<sizeof(TPage);i++)
+  for(i=0;i<sizeof(TActionPage);i++)
     ((uint8_t *)page)[i]=0x00;
 }
 
-void pages_copy_page(TPage *src,TPage *dst)
+void pages_copy_page(TActionPage *src,TActionPage *dst)
 {
   uint16_t i=0;
 
-  for(i=0;i<sizeof(TPage);i++)
+  for(i=0;i<sizeof(TActionPage);i++)
     ((uint8_t *)dst)[i]=((volatile uint8_t *)src)[i];
 }
 
-uint8_t load_page_info(uint8_t page_id,TPage *page) 
+uint8_t load_page_info(uint8_t page_id,TActionPage *page) 
 {
   uint32_t i=0;
   uint8_t *ppage = (uint8_t *)page;
 
-  for(i=0;i<sizeof(TPage);i++) 
-    ppage[i]= pgm_read_byte_far((uint32_t)PAGE_BASE_ADDR+((uint32_t)sizeof(TPage)*(uint32_t)page_id)+i);
+  for(i=0;i<sizeof(TActionPage);i++) 
+    ppage[i]= pgm_read_byte_far((uint32_t)PAGE_BASE_ADDR+((uint32_t)sizeof(TActionPage)*(uint32_t)page_id)+i);
   
   if(pages_check_checksum(page)==0) 
     return 0;