diff --git a/Makefile b/Makefile
index 456e829d05af9a25d1f09e9ae62ceb8f34d9c43d..ec863f8dbd6ab43f664cb33dd39e98b5b0f575db 100755
--- a/Makefile
+++ b/Makefile
@@ -34,7 +34,7 @@ DYNAMIXEL_PATH=../../STM32_processor/libraries/dynamixel_base
 BUILD_PATH=build
 
 COMPILE_OPTS = -mlittle-endian -mcpu=cortex-m4 -mthumb -mthumb-interwork 
-COMPILE_OPTS += -Wall -O2 -fno-common 
+COMPILE_OPTS += -Wall -O2 -fno-common -DUSE_HAL_DRIVER 
 COMPILE_OPTS += -mfloat-abi=hard -mfpu=fpv4-sp-d16
 COMPILE_OPTS += -ffreestanding -nostdlib -D$(PROCESSOR_MACRO) -DARM_MATH_CM4 -D__FPU_PRESENT -DHSE_VALUE=8000000
 INCLUDE_DIRS = -I$(HAL_PATH)/include -I$(HAL_PATH)/include/core -I$(HAL_PATH)/include/devices
diff --git a/include/action.h b/include/action.h
index 3a5dcb4d8dff7d65a4e7718463baf24b9d33bfb3..693ccc167a525e600e8c1f30b103b23f593bf394 100644
--- a/include/action.h
+++ b/include/action.h
@@ -5,7 +5,7 @@
 extern "C" {
 #endif
 
-#include "stm32f4xx_hal.h"
+#include "stm32f4xx.h"
 #include "motion_pages.h"
 
 extern int64_t action_current_angles[PAGE_MAX_NUM_SERVOS];
diff --git a/include/action_id.h b/include/action_id.h
index bedb456c33964ba8fb1d6ae078aa1dc60abe8f45..10c36280ec7962672eb397fab14b03b0e67b370f 100644
--- a/include/action_id.h
+++ b/include/action_id.h
@@ -1,6 +1,10 @@
 #ifndef _ACTION_ID_H
 #define _ACTION_ID_H
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+
 #define     BOW               1
 #define     BRAVO             2
 #define     RAP_CHEST         5
@@ -137,4 +141,8 @@
 #define     BALANCE           224
 #define     CLAP_END          225
 
+#ifdef __cplusplus
+}
+#endif
+
 #endif 
diff --git a/include/adc_dma.h b/include/adc_dma.h
index 35a7b0ba81d3ce3ede7031ef254e38ccfb5925a7..d360ab07c9f44fb271a20ce56d27876e82714ec5 100644
--- a/include/adc_dma.h
+++ b/include/adc_dma.h
@@ -5,7 +5,7 @@
 extern "C" {
 #endif
 
-#include "stm32f4xx_hal.h"
+#include "stm32f4xx.h"
 
 typedef enum {ADC_CH1,ADC_CH2,ADC_CH3,ADC_CH4,ADC_CH6,ADC_CH8} adc_ch_t;
 
diff --git a/include/bioloid_dyn_master_sensors.h b/include/bioloid_dyn_master_sensors.h
index db8a6c449dfc14fe3f811c010fa71949f36a8642..98554dc5d42abe194303cddb7e9b1fb5347257ab 100644
--- a/include/bioloid_dyn_master_sensors.h
+++ b/include/bioloid_dyn_master_sensors.h
@@ -1,12 +1,20 @@
 #ifndef _BIOLOID_DYN_MASTER_SENSORS_H
 #define _BIOLOID_DYN_MASTER_SENSORS_H
 
-#include "stm32f4xx_hal.h"
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "stm32f4xx.h"
 #include "stm32_time.h"
 #include "dynamixel_master.h"
 #include "comm.h"
 
 void bioloid_dyn_master_sensors_init(void);
 
+#ifdef __cplusplus
+}
+#endif
+
 #endif
 
diff --git a/include/bioloid_dyn_master_servos.h b/include/bioloid_dyn_master_servos.h
index b512dd298790dfa808f78861b7e3b6c09021b49c..3152d5673ce5a3eb8b9d2de52aebf0bfa4d961d1 100644
--- a/include/bioloid_dyn_master_servos.h
+++ b/include/bioloid_dyn_master_servos.h
@@ -1,8 +1,11 @@
 #ifndef _BIOLOID_DYN_MASTER_SERVOS_H
 #define _BIOLOID_DYN_MASTER_SERVOS_H
 
-#include "stm32f4xx_hal.h"
-#include "stm32_time.h"
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "stm32f4xx.h"
 #include "dynamixel_master.h"
 #include "comm.h"
 
@@ -12,5 +15,9 @@ void bioloid_dyn_master_servos_init(void);
 inline void bioloid_dyn_master_servos_enable_power(void);
 inline void bioloid_dyn_master_servos_disable_power(void);
 
+#ifdef __cplusplus
+}
+#endif
+
 #endif
 
diff --git a/include/bioloid_dyn_slave.h b/include/bioloid_dyn_slave.h
index f6d7158fffbbefc629000aafac9d2dada26f50f7..9ceda0a3a1db36797871db26c9d4685abde1f811 100644
--- a/include/bioloid_dyn_slave.h
+++ b/include/bioloid_dyn_slave.h
@@ -5,8 +5,7 @@
 extern "C" {
 #endif
 
-#include "stm32f4xx_hal.h"
-#include "stm32_time.h"
+#include "stm32f4xx.h"
 #include "dynamixel_slave.h"
 #include "comm.h"
 
diff --git a/include/bioloid_gyro.h b/include/bioloid_gyro.h
index 62074964f49e8c5504357aa4fd22a827e04d2e34..2b7f9997289130e4598bf99d222931a0576a34c2 100644
--- a/include/bioloid_gyro.h
+++ b/include/bioloid_gyro.h
@@ -1,7 +1,11 @@
 #ifndef BIOLOID_GYRO_H
 #define BIOLOID_GYRO_H
 
-#include "stm32f4xx_hal.h"
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "stm32f4xx.h"
 #include "adc_dma.h"
 
 /* public functions */
@@ -15,4 +19,8 @@ uint8_t gyro_in_range(unsigned short int address, unsigned short int length);
 void gyro_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
 void gyro_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
 
+#ifdef __cplusplus
+}
+#endif
+
 #endif
diff --git a/include/bioloid_time.h b/include/bioloid_time.h
index 13ba01bbe4fd47210059324744657d0fd5f9314f..ccc113bc306909090b4b4507d1a3727a10084ce5 100755
--- a/include/bioloid_time.h
+++ b/include/bioloid_time.h
@@ -5,7 +5,7 @@
 extern "C" {
 #endif
 
-#include "stm32f4xx_hal.h"
+#include "stm32f4xx.h"
 
 void bioloid_time_init(void);
 unsigned long long int bioloid_time_get_counts(void);
diff --git a/include/eeprom.h b/include/eeprom.h
index 25c56092a20b8b2c8795c3a052b5c87ca371016c..d1edf80f83f8aea550f16274e909670a5c7111e5 100755
--- a/include/eeprom.h
+++ b/include/eeprom.h
@@ -40,8 +40,12 @@
 #ifndef __EEPROM_H
 #define __EEPROM_H
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+
 /* Includes ------------------------------------------------------------------*/
-#include "stm32f4xx_hal.h"
+#include "stm32f4xx.h"
 
 /* Exported constants --------------------------------------------------------*/
 /* Define the size of the sectors to be used */
@@ -88,6 +92,10 @@ uint16_t EE_Init(void);
 uint16_t EE_ReadVariable(uint16_t VirtAddress, uint16_t* Data);
 uint16_t EE_WriteVariable(uint16_t VirtAddress, uint16_t Data);
 
+#ifdef __cplusplus
+}
+#endif
+
 #endif /* __EEPROM_H */
 
 /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
diff --git a/include/gpio.h b/include/gpio.h
index 76907b0abb8222c682ee649ab5aebd5c70ef96f3..2cc86d3c84b010b5d49cae1d0e585d77820539bb 100644
--- a/include/gpio.h
+++ b/include/gpio.h
@@ -5,7 +5,7 @@
 extern "C" {
 #endif
 
-#include "stm32f4xx_hal.h"
+#include "stm32f4xx.h"
 
 typedef enum {USER1_LED,USER2_LED,RXD_LED,TXD_LED} led_t;
 
diff --git a/include/motion_manager.h b/include/motion_manager.h
index 3a1172a98a5fab9fb4db683993a1aee323eb00bc..e6a1616b2776b95096e80f7e77504eba4cf237dc 100644
--- a/include/motion_manager.h
+++ b/include/motion_manager.h
@@ -5,7 +5,7 @@
 extern "C" {
 #endif
 
-#include "stm32f4xx_hal.h"
+#include "stm32f4xx.h"
 
 typedef enum {
   R_SHOULDER_PITCH     = 1,
diff --git a/include/motion_pages.h b/include/motion_pages.h
index 5ff494fad35ea7b9892fe3d3851c9c8b141dace8..a195e1705a164993678da09238ef0047fb8a1a95 100644
--- a/include/motion_pages.h
+++ b/include/motion_pages.h
@@ -5,7 +5,7 @@
 extern "C" {
 #endif
 
-#include "stm32f4xx_hal.h"
+#include "stm32f4xx.h"
 
 #define MAX_PAGES                     256 
 #define PAGE_MAX_NUM_SERVOS           31
diff --git a/include/zigbee.h b/include/zigbee.h
index 87526c65f0d1b2efe76ea77fe57ee64f8fa755ef..8672f7b56187784e9ff6af2838503d6cc56facee 100644
--- a/include/zigbee.h
+++ b/include/zigbee.h
@@ -5,7 +5,7 @@
 extern "C" {
 #endif
 
-#include "stm32f4xx_hal.h"
+#include "stm32f4xx.h"
 
 // public functions
 void zigbee_init(void);
diff --git a/src/motion_manager.c b/src/motion_manager.c
index 1c430c285ccb92951665886f790483d085a392c3..4370b172836180863b4663148d4a8c9f85a293f6 100644
--- a/src/motion_manager.c
+++ b/src/motion_manager.c
@@ -184,7 +184,7 @@ void manager_init(uint16_t period_us)
   dyn_master_scan(&bioloid_dyn_master_servos,&num,servo_ids); 
   ram_data[BIOLOID_MM_NUM_SERVOS]=num;
   manager_num_servos=0;
-  for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
+/*  for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
   {
     if(i==servo_ids[manager_num_servos])
     {
@@ -289,7 +289,7 @@ void manager_init(uint16_t period_us)
       manager_servos[i].ccw_angle_limit=0;
     }
   }
-  bioloid_dyn_master_servos_disable_power();
+  bioloid_dyn_master_servos_disable_power();*/
 
   /* configure timer */
   ENABLE_MANAGER_TIMER_CLK;
@@ -298,29 +298,27 @@ void manager_init(uint16_t period_us)
   MANAGER_TIM_Handle.Init.Prescaler = 84;
   MANAGER_TIM_Handle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
   MANAGER_TIM_Handle.Init.CounterMode = TIM_COUNTERMODE_UP;
-  HAL_NVIC_SetPriority(MANAGER_TIMER_IRQn, 2, 1);
-  HAL_NVIC_EnableIRQ(MANAGER_TIMER_IRQn);
+//  HAL_NVIC_SetPriority(MANAGER_TIMER_IRQn, 2, 1);
+//  HAL_NVIC_EnableIRQ(MANAGER_TIMER_IRQn);
   /* use the internal clock */
-  sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
-  HAL_TIM_ConfigClockSource(&MANAGER_TIM_Handle, &sClockSourceConfig);
-  HAL_TIM_OC_Init(&MANAGER_TIM_Handle);
+//  sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
+//  HAL_TIM_ConfigClockSource(&MANAGER_TIM_Handle, &sClockSourceConfig);
+//  HAL_TIM_OC_Init(&MANAGER_TIM_Handle);
   /* disable master/slave mode */
-  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
-  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
-  HAL_TIMEx_MasterConfigSynchronization(&MANAGER_TIM_Handle, &sMasterConfig);
+//  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
+//  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
+//  HAL_TIMEx_MasterConfigSynchronization(&MANAGER_TIM_Handle, &sMasterConfig);
   /* configure ouptut counter channel 4 */
-  manager_motion_period=(period_us*1000000)>>16;
-  manager_motion_period_us=period_us;
+//  manager_motion_period=(period_us*1000000)>>16;
+//  manager_motion_period_us=period_us;
 
   /* initialize balance parameters */
-  for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
-    manager_balance_offset[i]=0;
-  manager_balance_enabled=0x00;
+//  for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
+//    manager_balance_offset[i]=0;
+//  manager_balance_enabled=0x00;
 
   /* initialize action module */
-  action_init(period_us);
-
-  gpio_blink_led(RXD_LED,1000);
+//  action_init(period_us);
 }
 
 uint16_t manager_get_period(void)