diff --git a/.gitmodules b/.gitmodules
new file mode 100644
index 0000000000000000000000000000000000000000..46f8d3e4ba76cc8f424f57b802f1c0da1595df16
--- /dev/null
+++ b/.gitmodules
@@ -0,0 +1,8 @@
+[submodule "stm32_libraries"]
+	path = stm32_libraries
+	url = ssh://git@gitlab.iri.upc.edu:2202/humanoides/tools/stm32_libraries.git
+	branch = master
+[submodule "stm32_hal"]
+	path = stm32_hal
+	url = ssh://git@gitlab.iri.upc.edu:2202/humanoides/tools/stm32_hal.git
+	branch = master
diff --git a/CMakeLists.txt b/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..81aee0df13d3acd93fc92c48056370a5c4783c3f
--- /dev/null
+++ b/CMakeLists.txt
@@ -0,0 +1,122 @@
+cmake_minimum_required(VERSION 3.5)
+
+PROJECT(darwin_stm32_fw C ASM)
+set(PROJECT_NAME darwin_stm32_fw)
+
+# driver source files
+SET(lib_sources ./stm32_libraries/utils/src/buffer.c 
+                ./stm32_libraries/utils/src/stm32_time.c
+                ./stm32_libraries/comm/src/comm.c
+                ./stm32_libraries/scheduler/src/scheduler.c
+                ./stm32_libraries/memory/src/mem_module.c 
+                ./stm32_libraries/memory/src/memory.c 
+                ./stm32_libraries/memory/src/ram.c
+                ./stm32_libraries/dynamixel_base/src/dynamixel2.c
+                ./stm32_libraries/dynamixel_base/src/dynamixel.c  
+                ./stm32_libraries/dynamixel_base/src/dynamixel_master.c
+                ./stm32_libraries/dynamixel_base/src/dynamixel_slave.c
+                ./stm32_libraries/dynamixel_base/src/dynamixel_slave_device.c
+                ./stm32_libraries/dynamixel_manager/src/dyn_manager.c
+                ./stm32_libraries/dynamixel_manager/src/dyn_module.c
+                ./stm32_libraries/dynamixel_manager/src/modules/motion_manager.c
+                ./stm32_libraries/dynamixel_manager/src/modules/motion_module.c
+                ./stm32_libraries/dynamixel_manager/src/modules/action.c
+                ./stm32_libraries/dynamixel_manager/src/modules/motion_pages.c
+                ./stm32_libraries/dynamixel_manager/src/modules/dyn_servos.c)
+# application header files
+SET(lib_headers ./stm32_libraries/utils/include/buffer.h 
+                ./stm32_libraries/utils/include/stm32_time.h
+                ./stm32_libraries/comm/include/comm.h
+                ./stm32_libraries/scheduler/include/scheduler.h
+                ./stm32_libraries/memory/include/mem_module.h 
+                ./stm32_libraries/memory/include/memory.h 
+                ./stm32_libraries/memory/include/ram.h
+                ./stm32_libraries/dynamixel_base/include/dynamixel2.h
+                ./stm32_libraries/dynamixel_base/include/dynamixel.h
+                ./stm32_libraries/dynamixel_base/include/dynamixel_master.h
+                ./stm32_libraries/dynamixel_base/include/dynamixel_slave_device.h
+                ./stm32_libraries/dynamixel_base/include/dynamixel_slave.h
+                ./stm32_libraries/dynamixel_base/include/dynamixel_slave_registers.h
+                ./stm32_libraries/dynamixel_base/include/dyn_common.h
+                ./stm32_libraries/dynamixel_manager/include/dyn_manager.h
+                ./stm32_libraries/dynamixel_manager/include/dyn_manager_registers.h
+                ./stm32_libraries/dynamixel_manager/include/dyn_module.h
+                ./stm32_libraries/dynamixel_manager/include/dyn_module_registers.h
+                ./stm32_libraries/dynamixel_manager/include/dyn_devices.h
+                ./stm32_libraries/dynamixel_manager/include/modules/motion_manager.h
+                ./stm32_libraries/dynamixel_manager/include/modules/motion_manager_registers.h
+                ./stm32_libraries/dynamixel_manager/include/modules/motion_module.h
+                ./stm32_libraries/dynamixel_manager/include/modules/action.h
+                ./stm32_libraries/dynamixel_manager/include/modules/motion_pages.h
+                ./stm32_libraries/dynamixel_manager/include/modules/dyn_servos.h)
+
+# add include directories
+include_directories(./stm32_libraries/utils/include)
+include_directories(./stm32_libraries/comm/include)
+include_directories(./stm32_libraries/scheduler/include)
+include_directories(./stm32_libraries/memory/include)
+include_directories(./stm32_libraries/dynamixel_base/include)
+include_directories(./stm32_libraries/dynamixel_manager/include)
+include_directories(./stm32_libraries/dynamixel_manager/include/modules)
+
+set(hal_sources ./stm32_hal/f1/src/stm32f1xx_hal_gpio.c
+                ./stm32_hal/f1/src/stm32f1xx_hal_cortex.c
+                ./stm32_hal/f1/src/stm32f1xx_hal_rcc.c
+                ./stm32_hal/f1/src/stm32f1xx_hal_rcc_ex.c
+                ./stm32_hal/f1/src/stm32f1xx_hal_pwr.c
+                ./stm32_hal/f1/src/stm32f1xx_hal_tim.c
+                ./stm32_hal/f1/src/stm32f1xx_hal_tim_ex.c
+                ./stm32_hal/f1/src/stm32f1xx_hal_dma.c
+                ./stm32_hal/f1/src/stm32f1xx_hal_usart.c
+                ./stm32_hal/f1/src/stm32f1xx_hal_flash.c
+                ./stm32_hal/f1/src/stm32f1xx_hal_flash_ex.c
+                ./stm32_hal/f1/src/stm32f1xx_hal_adc.c
+                ./stm32_hal/f1/src/stm32f1xx_hal_adc_ex.c
+                ./stm32_hal/f1/src/stm32f1xx_hal_spi.c
+                ./stm32_hal/f1/src/stm32f1xx_hal.c)
+
+include_directories(./stm32_hal/f1/include)
+include_directories(./stm32_hal/f1/include/core)
+include_directories(./stm32_hal/f1/include/devices)
+
+set(asm_source "./stm32_hal/f1/startup_code/startup_stm32f103xe.s")
+set_source_files_properties(${asm_sources} PROPERTIES COMPILE_FLAGS "-c ${CMAKE_C_FLAGS}")
+
+# cross-compile settings
+set(CMAKE_SYSTEM_NAME Linux)
+set(CMAKE_SYSTEM_PROCESSOR arm)
+set(CMAKE_C_COMPILER arm-none-eabi-gcc)
+set(CMAKE_OBJCOPY arm-none-eabi-objcopy)
+set(CMAKE_SIZE arm-none-eabi-size)
+set(CMAKE_ASM_COMPILER arm-none-eabi-gcc)
+
+# processor selection
+set(TARGET_PROCESSOR STM32F103RE)
+
+# compiler settings
+SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -mlittle-endian -mcpu=cortex-m3 -mthumb -mthumb-interwork -Wall -g -fno-common -msoft-float -ffreestanding -imacros ${CMAKE_CURRENT_SOURCE_DIR}/include/darwin_conf.h")
+
+# linker settings
+set(LINKER_SCRIPT "${CMAKE_CURRENT_SOURCE_DIR}/linker_script/darwin.ld")
+set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -Wl,-Map=${PROJECT_NAME}.map,-cref -T ${LINKER_SCRIPT} --specs=nosys.specs")
+
+ADD_SUBDIRECTORY(src)
+
+include_directories(${header_include_dir})
+
+add_definitions(-DUSE_HAL_DRIVER)
+add_definitions(-DHSE_VALUE=${system_freq})
+add_definitions(-D${processor})
+
+add_executable(${PROJECT_NAME}.elf ${lib_sources} ${hal_sources} ${asm_source} ${sources})
+
+add_custom_command(TARGET "${PROJECT_NAME}.elf" POST_BUILD
+        # Build .hex and .bin files
+        COMMAND ${CMAKE_OBJCOPY} -O binary ${PROJECT_NAME}.elf ${PROJECT_NAME}.bin
+        COMMAND ${CMAKE_OBJCOPY} -O ihex  ${PROJECT_NAME}.elf ${PROJECT_NAME}.hex
+        COMMENT "Building ${PROJECT_NAME}.bin and ${PROJECT_NAME}.hex"
+
+        # Display sizes
+        COMMAND ${CMAKE_SIZE} --format=berkeley ${PROJECT_NAME}.elf ${PROJECT_NAME}.hex
+        COMMENT "Invoking: Cross ARM GNU Print Size"
+        )
diff --git a/Makefile b/Makefile
deleted file mode 100755
index 79a4b64d626eb4c8f9e9a54172fd8ec8ce51c975..0000000000000000000000000000000000000000
--- a/Makefile
+++ /dev/null
@@ -1,158 +0,0 @@
-# setup
-# modified by zerom for WinARM 8/2010
-
-STM32_HAL_PATH=$(HOME)/humanoides/tools/stm32_hal
-STM32_LIBRARIES_PATH=$(HOME)/humanoides/tools/stm32_libraries
-
-PROJECT_NAME=darwin_firmware
-#TARGET_FILES=$(wildcard src/*.c)
-TARGET_FILES=src/cm730_fw.c
-TARGET_FILES+=src/eeprom.c
-TARGET_FILES+=src/system_stm32f1xx.c
-#TARGET_FILES+=src/gpio.c
-TARGET_FILES+=src/darwin_sch.c
-TARGET_FILES+=src/darwin_time.c
-#TARGET_FILES+=src/adc_dma.c
-#TARGET_FILES+=src/imu.c
-TARGET_FILES+=src/darwin_dyn_slave.c
-#TARGET_FILES+=src/darwin_dyn_master.c
-#TARGET_FILES+=src/darwin_dyn_master_v2.c
-TARGET_FILES+=src/stm32f1xx_hal_msp.c
-#TARGET_FILES+=src/motion_manager.c
-#TARGET_FILES+=src/action.c
-#TARGET_FILES+=src/motion_pages.c
-#TARGET_FILES+=src/walking.c
-#TARGET_FILES+=src/darwin_math.c
-#TARGET_FILES+=src/darwin_kinematics.c
-#TARGET_FILES+=src/joint_motion.c
-#TARGET_FILES+=src/head_tracking.c
-#TARGET_FILES+=src/grippers.c
-#TARGET_FILES+=src/smart_charger.c
-#TARGET_FILES+=src/stairs.c
-
-TARGET_PROCESSOR=STM32F103RE
-
-HAL_PATH=$(STM32_HAL_PATH)/f1
-
-include $(HAL_PATH)/select_processor.mk
-
-STM32_STARTUP_FILES_PATH = $(HAL_PATH)/startup_code/
-STM32_LINKER_SCRIPTS_PATH = ./linker_script
-UTILS_PATH=$(STM32_LIBRARIES_PATH)/utils
-COMM_PATH=$(STM32_LIBRARIES_PATH)/comm
-MEMORY_PATH=$(STM32_LIBRARIES_PATH)/memory
-USART_PATH=$(STM32_LIBRARIES_PATH)/f1/usart
-DYNAMIXEL_PATH=$(STM32_LIBRARIES_PATH)/dynamixel_base
-SCHEDULER_PATH=$(STM32_LIBRARIES_PATH)/scheduler
-BUILD_PATH=build
-
-COMPILE_OPTS = -mlittle-endian -mcpu=cortex-m3 -mthumb -mthumb-interwork 
-COMPILE_OPTS += -Wall -O2 -fno-common -msoft-float -DUSE_HAL_DRIVER
-COMPILE_OPTS += -ffreestanding -nostdlib -D$(PROCESSOR_MACRO) -DHSE_VALUE=8000000 -imacros ./include/darwin_conf.h
-INCLUDE_DIRS = -I$(HAL_PATH)/include -I$(HAL_PATH)/include/core -I$(HAL_PATH)/include/devices 
-INCLUDE_DIRS += -I$(UTILS_PATH)/include -I$(COMM_PATH)/include -I$(MEMORY_PATH)/include -I$(DYNAMIXEL_PATH)/include -I$(USART_PATH)/include -I$(SCHEDULER_PATH)/include -I./include
-
-DOC_DIR = ./doc
-
-TCHAIN_PREFIX=arm-none-eabi-
-
-CC = $(TCHAIN_PREFIX)gcc
-CFLAGS = $(COMPILE_OPTS) $(INCLUDE_DIRS) 
-
-AS = $(TCHAIN_PREFIX)gcc
-ASFLAGS = $(COMPILE_OPTS) -c 
-
-LD = $(TCHAIN_PREFIX)gcc
-LDFLAGS = -mthumb -mcpu=cortex-m3 -Wl,-Map=$@.map,-cref $(INCLUDE_DIRS) -T $(STM32_LINKER_SCRIPTS_PATH)/darwin.ld --specs=nosys.specs
-
-OBJCP = $(TCHAIN_PREFIX)objcopy
-OBJCPFLAGS_HEX = -O ihex
-OBJCPFLAGS_BIN = -O binary
-OBJDUMP = $(TCHAIN_PREFIX)objdump
-OBJDUMPFLAGS = -h -S -C -D
-
-MAIN_OUT_ELF = $(BUILD_PATH)/$(PROJECT_NAME).elf
-MAIN_OUT_HEX = $(BUILD_PATH)/$(PROJECT_NAME).hex
-MAIN_OUT_BIN = $(BUILD_PATH)/$(PROJECT_NAME).bin
-MAIN_OUT_LSS = $(BUILD_PATH)/$(PROJECT_NAME).lss
-
-# add STM32 src files
-TARGET_FILES+=$(HAL_PATH)/src/stm32f1xx_hal_gpio.c
-TARGET_FILES+=$(HAL_PATH)/src/stm32f1xx_hal_cortex.c
-TARGET_FILES+=$(HAL_PATH)/src/stm32f1xx_hal_rcc.c
-TARGET_FILES+=$(HAL_PATH)/src/stm32f1xx_hal_rcc_ex.c
-TARGET_FILES+=$(HAL_PATH)/src/stm32f1xx_hal_pwr.c
-TARGET_FILES+=$(HAL_PATH)/src/stm32f1xx_hal_tim.c
-TARGET_FILES+=$(HAL_PATH)/src/stm32f1xx_hal_tim_ex.c
-TARGET_FILES+=$(HAL_PATH)/src/stm32f1xx_hal_dma.c
-TARGET_FILES+=$(HAL_PATH)/src/stm32f1xx_hal_uart.c
-TARGET_FILES+=$(HAL_PATH)/src/stm32f1xx_hal_flash.c
-TARGET_FILES+=$(HAL_PATH)/src/stm32f1xx_hal_flash_ex.c
-TARGET_FILES+=$(HAL_PATH)/src/stm32f1xx_hal_adc.c
-TARGET_FILES+=$(HAL_PATH)/src/stm32f1xx_hal_adc_ex.c
-TARGET_FILES+=$(HAL_PATH)/src/stm32f1xx_hal_spi.c
-TARGET_FILES+=$(HAL_PATH)/src/stm32f1xx_hal.c
-
-TARGET_FILES+=$(USART_PATH)/src/usart3.c
-TARGET_FILES+=$(USART_PATH)/src/usart2.c
-TARGET_FILES+=$(USART_PATH)/src/usart1_remap.c
-
-TARGET_FILES+=$(wildcard $(UTILS_PATH)/src/*.c)
-TARGET_FILES+=$(wildcard $(COMM_PATH)/src/*.c)
-TARGET_FILES+=$(wildcard $(MEMORY_PATH)/src/*.c)
-TARGET_FILES+=$(DYNAMIXEL_PATH)/src/dynamixel2.c
-TARGET_FILES+=$(DYNAMIXEL_PATH)/src/dynamixel.c
-TARGET_FILES+=$(DYNAMIXEL_PATH)/src/dynamixel_slave.c
-TARGET_FILES+=$(DYNAMIXEL_PATH)/src/dynamixel_slave_device.c
-TARGET_FILES+=$(wildcard $(SCHEDULER_PATH)/src/*.c)
-
-DARWIN_OBJS_TMP = $(notdir $(TARGET_FILES:.c=.o))
-DARWIN_OBJS = $(patsubst %,$(BUILD_PATH)/%,$(DARWIN_OBJS_TMP))
-
-all: $(MAIN_OUT_ELF) $(MAIN_OUT_HEX) $(MAIN_OUT_BIN) $(MAIN_OUT_LSS)
-
-mkdir_build: 
-	mkdir -p $(BUILD_PATH)
-
-$(BUILD_PATH)/%.o: src/%.c 
-	$(CC) -c $(CFLAGS) -o $@ $<
-$(BUILD_PATH)/%.o: $(HAL_PATH)/src/devices/%.c
-	$(CC) -c $(CFLAGS) -o $@ $<
-$(BUILD_PATH)/%.o: $(HAL_PATH)/src/%.c
-	$(CC) -c $(CFLAGS) -o $@ $<
-$(BUILD_PATH)/%.o: $(USART_PATH)/src/%.c
-	$(CC) -c $(CFLAGS) -o $@ $<
-$(BUILD_PATH)/%.o: $(UTILS_PATH)/src/%.c
-	$(CC) -c $(CFLAGS) -o $@ $<
-$(BUILD_PATH)/%.o: $(COMM_PATH)/src/%.c
-	$(CC) -c $(CFLAGS) -o $@ $<
-$(BUILD_PATH)/%.o: $(MEMORY_PATH)/src/%.c
-	$(CC) -c $(CFLAGS) -o $@ $<
-$(BUILD_PATH)/%.o: $(DYNAMIXEL_PATH)/src/%.c
-	$(CC) -c $(CFLAGS) -o $@ $<
-$(BUILD_PATH)/%.o: $(SCHEDULER_PATH)/src/%.c
-	$(CC) -c $(CFLAGS) -o $@ $<
-
-$(MAIN_OUT_ELF): mkdir_build $(DARWIN_OBJS) $(BUID_PATH)/$(STARTUP_FILE:.s=.o) 
-	$(LD) $(LDFLAGS) $(DARWIN_OBJS) $(BUILD_PATH)/$(STARTUP_FILE:.s=.o) $(EXT_LIB) -lm --output $@
-
-$(MAIN_OUT_HEX): $(MAIN_OUT_ELF)	
-	$(OBJCP) $(OBJCPFLAGS_HEX) $< $@
-
-$(MAIN_OUT_BIN): $(MAIN_OUT_ELF)
-	$(OBJCP) $(OBJCPFLAGS_BIN) $< $@
-
-$(MAIN_OUT_LSS): $(MAIN_OUT_ELF)
-	$(OBJDUMP) $(OBJDUMPFLAGS) $< > $@
-
-$(BUID_PATH)/$(STARTUP_FILE:.s=.o): $(STM32_STARTUP_FILES_PATH)/$(STARTUP_FILE)
-	$(AS) $(ASFLAGS) -o $(BUILD_PATH)/$(STARTUP_FILE:.s=.o) $(STM32_STARTUP_FILES_PATH)/$(STARTUP_FILE) > $(BUILD_PATH)/$(STARTUP_FILE:.s=.lst)
-
-doc: 
-	doxygen $(DOC_DIR)/doxygen.conf 
-	
-clean:
-	-rm -rf $(BUILD_PATH)
-	-rm -rf doc/html
-
-.PHONY: all clean doc
diff --git a/include/darwin_conf.h b/include/darwin_conf.h
index 44ebbf8622d2d392c3cfbae49e8ccedf0e091c15..e3b83c3d0c9fe73e424b3a4ee53a94605c36b853 100644
--- a/include/darwin_conf.h
+++ b/include/darwin_conf.h
@@ -8,17 +8,29 @@
 #define EEPROM_DYN_SLAVE_BASE_ADDRESS1        ((unsigned short int)0x0000)
 #define EEPROM_DYN_SLAVE_BASE_ADDRESS2        ((unsigned short int)0x0010)
 
-#define DEFAULT_DEVICE_MODEL                  0x001D
-#define DEFAULT_FIRMWARE_VERSION              0x0001
-#define DEFAULT_DEVICE_ID                     0x0003
+#define DARWIN_DEVICE_MODEL                   0x001D
+#define DARWIN_FIRMWARE_VERSION               0x0001
+#define DARWIN_DEVICE_ID                      0x0003
 #define DEFAULT_BAUDRATE                      0x0022
 #define DEFAULT_RETURN_DELAY                  0x0000
 #define DEFAULT_RETURN_LEVEL                  0x0002
 
+#define NUM_MOTION_PAGES                      46
+#define MAX_DYN_MASTER_TX_BUFFER_LEN          1024
+#define MAX_DYN_MASTER_RX_BUFFER_LEN          1024
 #define MAX_DYN_SLAVE_TX_BUFFER_LEN           1024
 #define MAX_DYN_SLAVE_RX_BUFFER_LEN           1024
 #define MAX_DYN_SLAVE_REG_BUFFER_LEN          1024
 #define MAX_NUM_SLAVE_DEVICES                 4
+#define MAX_DYN_SLAVE_REG_BUFFER_LEN          1024
+#define DYN_MANAGER_MAX_NUM_MASTERS           4
+#define DYN_MANAGER_MAX_NUM_MODULES           8
+#define DYN_MANAGER_MAX_NUM_DEVICES           32
+#define DYN_MANAGER_MAX_NUM_SINGLE_OP         16
+#define DYN_MANAGER_MAX_NUM_SYNC_OP           4
+#define DYN_MANAGER_MAX_NUM_BULK_OP           4
+#define MODULE_MAX_NUM_MODELS                 32
+#define MM_MAX_NUM_MOTION_MODULES             8
 
 /* GPIO configuration */
 #define RAM_GPIO_BASE_ADDRESS                 ((unsigned short int)0x0100)
diff --git a/include/motion_pages.h b/include/motion_pages.h
deleted file mode 100755
index b4530de6acd85223c6c8763ca0786b75338b113a..0000000000000000000000000000000000000000
--- a/include/motion_pages.h
+++ /dev/null
@@ -1,59 +0,0 @@
-#ifndef _MOTION_PAGES_H
-#define _MOTION_PAGES_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#include "stm32f1xx.h"
-
-#define MAX_PAGES                     256 
-#define PAGE_MAX_NUM_SERVOS           31
-#define POSE_NUMBER_OF_POSES_PER_PAGE 7
-
-typedef struct // Header Structure (total 64unsigned char)
-{
-  uint8_t name[14];         // Name             0~13
-  uint8_t reserved1;        // Reserved1        14
-  uint8_t repeat;           // Repeat count     15
-  uint8_t schedule;         // schedule         16
-  uint8_t reserved2[3];     // reserved2        17~19
-  uint8_t stepnum;          // Number of step   20
-  uint8_t reserved3;        // reserved3        21
-  uint8_t speed;            // Speed            22
-  uint8_t reserved4;        // reserved4        23
-  uint8_t accel;            // Acceleration time 24
-  uint8_t next;             // Link to next     25
-  uint8_t exit;             // Link to exit     26
-  uint8_t reserved5[4];     // reserved5        27~30
-  uint8_t checksum;         // checksum         31
-  uint8_t slope[PAGE_MAX_NUM_SERVOS];        // CW/CCW compliance slope  32~62
-  uint8_t reserved6;        // reserved6        63
-} TPageHeader;
-
-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;
-
-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;
-
-extern TPage motion_pages[46];
-
-// 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);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif
diff --git a/include/stm32f1xx_hal_conf.h b/include/stm32f1xx_hal_conf.h
index 2625df4c9f900ba83e52d11bec8b8423a53c3bb0..5d51226505fd729dfd13e6ff4262ae9918cfb66c 100755
--- a/include/stm32f1xx_hal_conf.h
+++ b/include/stm32f1xx_hal_conf.h
@@ -80,8 +80,8 @@
 #define HAL_SPI_MODULE_ENABLED
 //#define HAL_SRAM_MODULE_ENABLED
 #define HAL_TIM_MODULE_ENABLED
-#define HAL_UART_MODULE_ENABLED
-//#define HAL_USART_MODULE_ENABLED
+//#define HAL_UART_MODULE_ENABLED
+#define HAL_USART_MODULE_ENABLED
 //#define HAL_WWDG_MODULE_ENABLED
 
 /* ########################## Oscillator Values adaptation ####################*/
diff --git a/linker_script/darwin.ld b/linker_script/darwin.ld
index 8aba769649d374f6be65ee522cb12d639795732b..61df86908ccfe954872d57e5a0513efb506517bc 100755
--- a/linker_script/darwin.ld
+++ b/linker_script/darwin.ld
@@ -42,6 +42,13 @@ SECTIONS
     . = ALIGN(4);
   } >EEPROM
 
+  .pages :
+  {
+    . = ALIGN(4);
+    *(.pages)
+    . = ALIGN(4);
+  } >PAGES
+
   /* The program code and other data goes into FLASH */
   .text :
   {
@@ -111,13 +118,6 @@ SECTIONS
     _edata = .;        /* define a global symbol at data end */
   } >RAM AT> FLASH
 
-  .pages :
-  {
-    . = ALIGN(4);
-    *(.pages)
-    . = ALIGN(4);
-  } >PAGES
-
   /* Uninitialized data section */
   . = ALIGN(4);
   .bss :
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..7d3b8172bcb8656128cff03f278a70b7794b8bb2
--- /dev/null
+++ b/src/CMakeLists.txt
@@ -0,0 +1,30 @@
+SET(sources ${CMAKE_CURRENT_SOURCE_DIR}/cm730_fw.c
+            ${CMAKE_CURRENT_SOURCE_DIR}/eeprom.c
+            ${CMAKE_CURRENT_SOURCE_DIR}/system_stm32f1xx.c
+            #${CMAKE_CURRENT_SOURCE_DIR}/gpio.c
+            ${CMAKE_CURRENT_SOURCE_DIR}/darwin_sch.c
+            ${CMAKE_CURRENT_SOURCE_DIR}/darwin_time.c
+            #${CMAKE_CURRENT_SOURCE_DIR}/adc_dma.c
+            #${CMAKE_CURRENT_SOURCE_DIR}/imu.c
+            ${CMAKE_CURRENT_SOURCE_DIR}/darwin_dyn_slave.c
+            #${CMAKE_CURRENT_SOURCE_DIR}/darwin_dyn_master.c
+            #${CMAKE_CURRENT_SOURCE_DIR}/darwin_dyn_master_v2.c
+            ${CMAKE_CURRENT_SOURCE_DIR}/stm32f1xx_hal_msp.c
+            #${CMAKE_CURRENT_SOURCE_DIR}/motion_manager.c
+            #${CMAKE_CURRENT_SOURCE_DIR}/action.c
+            #${CMAKE_CURRENT_SOURCE_DIR}/motion_pages.c
+            #${CMAKE_CURRENT_SOURCE_DIR}/walking.c
+            #${CMAKE_CURRENT_SOURCE_DIR}/darwin_math.c
+            #${CMAKE_CURRENT_SOURCE_DIR}/darwin_kinematics.c
+            #${CMAKE_CURRENT_SOURCE_DIR}/joint_motion.c
+            #${CMAKE_CURRENT_SOURCE_DIR}/head_tracking.c
+            #${CMAKE_CURRENT_SOURCE_DIR}/grippers.c
+            #${CMAKE_CURRENT_SOURCE_DIR}/smart_charger.c
+            #${CMAKE_CURRENT_SOURCE_DIR}/stairs.c
+            ${CMAKE_CURRENT_SOURCE_DIR}/../stm32_libraries/f1/usart/src/usart3.c
+            ${CMAKE_CURRENT_SOURCE_DIR}/../stm32_libraries/f1/usart/src/usart1.c PARENT_SCOPE)
+
+set(header_include_dir ${CMAKE_CURRENT_SOURCE_DIR}/../include ${CMAKE_CURRENT_SOURCE_DIR}/../stm32_libraries/f1/usart/include PARENT_SCOPE)
+
+set(processor STM32F103xE PARENT_SCOPE)
+set(system_freq 8000000 PARENT_SCOPE)
diff --git a/src/cm730_fw.c b/src/cm730_fw.c
index 326cbe94588553ddb046f74b1512287e9b9d9947..83fe297822aef0a93db74aef6f03e624fcfe2c45 100755
--- a/src/cm730_fw.c
+++ b/src/cm730_fw.c
@@ -25,6 +25,8 @@ int main(void)
   HAL_Init();
   /* initialize EEPROM */
   EE_Init();
+  // initialize time module
+  darwin_time_init();
   /* initialize the scheduler */
   scheduler=darwin_sch_init();
   /* initialize the GPIO module */
@@ -33,12 +35,8 @@ int main(void)
 //  adc_init(&darwin_memory);
   // initialize imu
 //  imu_init(&darwin_memory);
-  // initialize time module
-  darwin_time_init();
   /* initialize the dynamixel slave interface */
   darwin_dyn_slave_init(&darwin_memory,scheduler);
-  darwin_memory->eeprom_write_data=EE_WriteVariable;
-  darwin_memory->eeprom_read_data=EE_ReadVariable;
   /* initialize motion manager module */
 //  EE_ReadVariable(MM_PERIOD_OFFSET,&eeprom_data);
 //  period=eeprom_data&0x00FF;
diff --git a/src/darwin_dyn_slave.c b/src/darwin_dyn_slave.c
index 09c22bc7de4c1b161e080f2d0b1a01071e5f7ab3..778f080718f48a7b54c2c76ade750244db751d2c 100755
--- a/src/darwin_dyn_slave.c
+++ b/src/darwin_dyn_slave.c
@@ -1,6 +1,7 @@
 #include "darwin_dyn_slave.h"
 #include "darwin_time.h"
 #include "usart3.h"
+#include "eeprom.h"
 #include "ram.h"
 
 /* private variables */
@@ -8,14 +9,14 @@ TDynamixelSlave darwin_dyn_slave;
 TDynamixelSlaveDevice darwin_dyn_slave_device;
 TTime darwin_dyn_slave_timer;
 TComm darwin_dyn_slave_comm;
-UART_InitTypeDef darwin_comm_init;
+USART_InitTypeDef darwin_comm_init;
 TScheduler *darwin_dyn_slave_sch;
 
 /* memory module */
 TMemory *darwin_dyn_slave_memory;
 
 /* eeprom data */
-dyn_slave_control_eeprom_data(darwin_dyn_slave,".eeprom",EEPROM_DYN_SLAVE_BASE_ADDRESS1,EEPROM_DYN_SLAVE_BASE_ADDRESS2);
+dyn_slave_control_eeprom_data(example_dyn_slave,".eeprom",EEPROM_DYN_SLAVE_BASE_ADDRESS1,EEPROM_DYN_SLAVE_BASE_ADDRESS2,DARWIN_DEVICE_MODEL,DARWIN_FIRMWARE_VERSION,DARWIN_DEVICE_ID,DEFAULT_BAUDRATE,DEFAULT_RETURN_DELAY,DEFAULT_RETURN_LEVEL);
 
 // private functions
 unsigned char darwin_on_read(unsigned short int address,unsigned short int length,unsigned char *data)
@@ -47,13 +48,14 @@ uint8_t darwin_dyn_slave_init(TMemory **memory,TScheduler *scheduler)
 
   /* initialize the comm object */
   comm_init(&darwin_dyn_slave_comm,0x01,&darwin_dyn_slave_timer);
-  darwin_comm_init.BaudRate     = 2000000/(ram_data[DEFAULT_BAUDRATE]+1);
-  darwin_comm_init.WordLength   = UART_WORDLENGTH_8B;
-  darwin_comm_init.StopBits     = UART_STOPBITS_1;
-  darwin_comm_init.Parity       = UART_PARITY_NONE;
-  darwin_comm_init.Mode         = UART_MODE_TX_RX;
-  darwin_comm_init.HwFlowCtl    = UART_HWCONTROL_NONE;
-  darwin_comm_init.OverSampling = UART_OVERSAMPLING_16;
+  darwin_comm_init.BaudRate     = 2000000/(DEFAULT_BAUDRATE+1);
+  darwin_comm_init.WordLength   = USART_WORDLENGTH_8B;
+  darwin_comm_init.StopBits     = USART_STOPBITS_1;
+  darwin_comm_init.Parity       = USART_PARITY_NONE;
+  darwin_comm_init.Mode         = USART_MODE_TX_RX;
+  darwin_comm_init.CLKPolarity  = USART_POLARITY_LOW;
+  darwin_comm_init.CLKPhase     = USART_PHASE_1EDGE;
+  darwin_comm_init.CLKLastBit   = USART_LASTBIT_DISABLE;
   
   priorities.irq_priority=0;
   priorities.irq_subpriority=0;
@@ -64,7 +66,7 @@ uint8_t darwin_dyn_slave_init(TMemory **memory,TScheduler *scheduler)
 
   usart3_init(&darwin_dyn_slave_comm,&darwin_comm_init,&priorities);
   // initialize slave device
-  dyn_slave_device_init(&darwin_dyn_slave_device,DEFAULT_DEVICE_ID,EEPROM_DYN_SLAVE_BASE_ADDRESS1,EEPROM_DYN_SLAVE_BASE_ADDRESS2);
+  dyn_slave_device_init(&darwin_dyn_slave_device,DARWIN_DEVICE_ID,EEPROM_DYN_SLAVE_BASE_ADDRESS1,EEPROM_DYN_SLAVE_BASE_ADDRESS2);
   darwin_dyn_slave_device.on_read=darwin_on_read;
   darwin_dyn_slave_device.on_write=darwin_on_write;
   darwin_dyn_slave_device.on_ping=darwin_on_ping;
@@ -72,6 +74,8 @@ uint8_t darwin_dyn_slave_init(TMemory **memory,TScheduler *scheduler)
   dyn_slave_device_set_return_level(&darwin_dyn_slave_device,DEFAULT_RETURN_LEVEL);
   (*memory)=dyn_slave_device_get_memory(&darwin_dyn_slave_device);
   darwin_dyn_slave_memory=dyn_slave_device_get_memory(&darwin_dyn_slave_device);
+  darwin_dyn_slave_memory->eeprom_write_data=EE_WriteVariable;
+  darwin_dyn_slave_memory->eeprom_read_data=EE_ReadVariable;
 
   status=dyn_slave_init(&darwin_dyn_slave,&darwin_dyn_slave_comm,scheduler,SCHED_CH1,DYN_VER2);
   darwin_dyn_slave.set_baudrate=usart3_set_baudrate;
diff --git a/src/motion_pages.c b/src/motion_pages.c
index 264a213a12b335bfdbe1c9da928eeb4333b06e8a..7f456bc9ac718d40061d927d6d8d94ac58d76322 100755
--- a/src/motion_pages.c
+++ b/src/motion_pages.c
@@ -258,44 +258,7 @@ unsigned char page_map[MAX_PAGES]={
   0,
   0};
 
-void pages_get_page(uint8_t page_id,TPage *page)
-{
-  uint16_t i=0;
-
-  for(i=0;i<sizeof(TPage);i++)
-    ((uint8_t *)page)[i]=((__IO uint8_t *)&motion_pages[page_map[page_id]])[i];
-}
-
-uint8_t pages_check_checksum(TPage *page)
-{
-  uint8_t checksum=0x00;
-  uint16_t i=0;
-
-  for(i=0;i<sizeof(TPage);i++)
-    checksum+=((uint8_t *)page)[i];
-  if(checksum==0x00)
-    return 0x01;
-  else
-    return 0x00;
-}
-
-void pages_clear_page(TPage *page)
-{
-  uint16_t i=0;
-
-  for(i=0;i<sizeof(TPage);i++)
-    ((uint8_t *)page)[i]=0x00;
-}
-
-void pages_copy_page(TPage *src,TPage *dst)
-{
-  uint16_t i=0;
-
-  for(i=0;i<sizeof(TPage);i++)
-    ((uint8_t *)dst)[i]=((uint8_t *)src)[i];
-}
-
-TPage motion_pages[46] __attribute__ ((section (".pages")))=
+TPage motion_pages[NUM_MOTION_PAGES] __attribute__ ((section (".pages")))=
 {
   // page 0
   {
diff --git a/stm32_hal b/stm32_hal
new file mode 160000
index 0000000000000000000000000000000000000000..6b641abc9b688cc560e28a1b4e19f14beee80ede
--- /dev/null
+++ b/stm32_hal
@@ -0,0 +1 @@
+Subproject commit 6b641abc9b688cc560e28a1b4e19f14beee80ede
diff --git a/stm32_libraries b/stm32_libraries
new file mode 160000
index 0000000000000000000000000000000000000000..437588fc6a87b1cfff7aab5c0d2eaf93999afeee
--- /dev/null
+++ b/stm32_libraries
@@ -0,0 +1 @@
+Subproject commit 437588fc6a87b1cfff7aab5c0d2eaf93999afeee