diff --git a/Makefile b/Makefile
index 9e8847e6eecc065157e22f4e0db8a8defb63b734..3d7e910ef731c963b9ecdc94dabfbdc99a8151f8 100755
--- a/Makefile
+++ b/Makefile
@@ -29,8 +29,7 @@ DYNAMIXEL_PATH=$(STM32_LIBRARIES_PATH)/dynamixel_base
 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 += -Wall -g -fno-common -msoft-float -DUSE_HAL_DRIVER
+COMPILE_OPTS += -Wall -fno-common -msoft-float -DUSE_HAL_DRIVER
 COMPILE_OPTS += -ffreestanding -nostdlib -D$(PROCESSOR_MACRO) -DHSE_VALUE=8000000 
 INCLUDE_DIRS = -I$(HAL_PATH)/include -I$(HAL_PATH)/include/core -I$(HAL_PATH)/include/devices -I$(HAL_PATH)/include/legacy
 INCLUDE_DIRS += -I$(UTILS_PATH)/include -I$(COMM_PATH)/include -I$(DYNAMIXEL_PATH)/include -I$(MEMORY_PATH)/include -I$(USART_PATH)/include -I./include
@@ -45,7 +44,8 @@ ASFLAGS = $(COMPILE_OPTS) -c
 
 LD = $(TCHAIN_PREFIX)gcc
 LDFLAGS = -mthumb -mcpu=cortex-m3 -Wl,-Map=$@.map,-cref $(INCLUDE_DIRS) -T $(STM32_LINKER_SCRIPTS_PATH)/ir_feet.ld --specs=nosys.specs
-EXT_LIB = $(COMM_PATH)/lib/comm_m3.a $(UTILS_PATH)/lib/utils_m3.a $(DYNAMIXEL_PATH)/lib/dynamixel_m3.a $(MEMORY_PATH)/lib/memory_m3.a
+#EXT_LIB = $(COMM_PATH)/lib/comm_m3.a $(UTILS_PATH)/lib/utils_m3.a $(DYNAMIXEL_PATH)/lib/dynamixel_m3.a $(MEMORY_PATH)/lib/memory_m3.a
+EXT_LIB = $(COMM_PATH)/lib/comm_m3.a $(UTILS_PATH)/lib/utils_m3.a $(MEMORY_PATH)/lib/memory_m3.a
 
 OBJCP = $(TCHAIN_PREFIX)objcopy
 OBJCPFLAGS_HEX = -O ihex
@@ -75,7 +75,10 @@ TARGET_FILES+=$(HAL_PATH)/src/stm32f1xx_hal_dma.c
 TARGET_FILES+=$(HAL_PATH)/src/stm32f1xx_hal_adc.c
 TARGET_FILES+=$(HAL_PATH)/src/stm32f1xx_hal_adc_ex.c
 
-TARGET_FILES+=$(USART_PATH)/src/usart1_remap.c
+TARGET_FILES+=$(USART_PATH)/src/usart1.c
+TARGET_FILES+=$(DYNAMIXEL_PATH)/src/dynamixel.c
+TARGET_FILES+=$(DYNAMIXEL_PATH)/src/dynamixel2.c
+TARGET_FILES+=$(DYNAMIXEL_PATH)/src/dynamixel_slave.c
 
 IR_FEET_OBJS_TMP = $(notdir $(TARGET_FILES:.c=.o))
 IR_FEET_OBJS = $(patsubst %,$(BUILD_PATH)/%,$(IR_FEET_OBJS_TMP))
@@ -86,13 +89,15 @@ mkdir_build:
 	mkdir -p $(BUILD_PATH)
 
 $(BUILD_PATH)/%.o: src/%.c
-	$(CC) -c $(CFLAGS) -o $@ $<
+	$(CC) -c $(CFLAGS) -O2 -o $@ $<
 $(BUILD_PATH)/%.o: $(HAL_PATH)/src/devices/%.c
-	$(CC) -c $(CFLAGS) -o $@ $<
+	$(CC) -c $(CFLAGS) -O2 -o $@ $<
 $(BUILD_PATH)/%.o: $(HAL_PATH)/src/%.c
-	$(CC) -c $(CFLAGS) -o $@ $<
+	$(CC) -c $(CFLAGS) -O2 -o $@ $<
 $(BUILD_PATH)/%.o: $(USART_PATH)/src/%.c
-	$(CC) -c $(CFLAGS) -o $@ $<
+	$(CC) -c $(CFLAGS) -O2 -o $@ $<
+$(BUILD_PATH)/%.o: $(DYNAMIXEL_PATH)/src/%.c
+	$(CC) -c $(CFLAGS) -O2 -o $@ $<
 
 $(MAIN_OUT_ELF): mkdir_build $(IR_FEET_OBJS) $(BUID_PATH)/$(STARTUP_FILE:.s=.o) 
 	$(LD) $(LDFLAGS) $(IR_FEET_OBJS) $(BUILD_PATH)/$(STARTUP_FILE:.s=.o) $(EXT_LIB) --output $@
diff --git a/include/adc_dma.h b/include/adc_dma.h
index 6044a66e9dbe93c23feec4cb09cf9c4eb3db482e..014d0c5c0b2f2ff593c3558b417e1661da97e152 100755
--- a/include/adc_dma.h
+++ b/include/adc_dma.h
@@ -2,12 +2,17 @@
 #define _ADC_DMA_H
 
 #include "stm32f1xx.h"
+#include "memory.h"
 
 #define ADC_NUM_CHANNELS      10
 
-typedef enum {ADC_CH1=0,ADC_CH2,ADC_CH3,ADC_CH4,ADC_CH5,ADC_CH6,ADC_CH7,ADC_CH8,ADC_CH9,ADC_CH10} adc_dma_ch_t;
+typedef enum {DOWN_LEFT_MIDDLE_CH=0,DOWN_LEFT_REAR_CH,DOWN_ANALOG_CH,DOWN_LEFT_FRONT_CH,
+              DOWN_RIGHT_REAR_CH,DOWN_RIGHT_MIDDLE_CH,DOWN_RIGHT_FRONT_CH,
+              FRONT_LEFT_CH,FRONT_RIGHT_CH,FRONT_ANALOG_CH} adc_dma_ch_t;
 
+uint8_t adc_init(TMemory *memory);
 float adc_get_voltage(adc_dma_ch_t channel_id);
 inline float adc_get_temperature(void);
+inline float adc_ref_voltage(void);
 
 #endif
diff --git a/include/adc_dma_registers.h b/include/adc_dma_registers.h
index c1cb1da812cc9699f2d5fe01c6481f85b0820962..c59db49d3669af77a072929510b3db6a2845a0e0 100644
--- a/include/adc_dma_registers.h
+++ b/include/adc_dma_registers.h
@@ -7,23 +7,20 @@
   #define RAM_ADC_DMA_BASE_ADDRESS            ((unsigned short int)0x0000)
 #endif
 
-#ifndef EEPROM_ADC_DMA_BASE_ADDRESS
-  #define EEPROM_ADC_DMA_BASE_ADDRESS         ((unsigned short int)0x0000)
-#endif
-
-#define RAM_ADC_DMA_LENGTH                    22
+#define RAM_ADC_DMA_LENGTH                    24
 
-#define ADC_CH1_VOLTAGE                       RAM_ADC_DMA_BASE_ADDRESS
-#define ADC_CH2_VOLTAGE                       (RAM_ADC_DMA_BASE_ADDRESS+2)
-#define ADC_CH3_VOLTAGE                       (RAM_ADC_DMA_BASE_ADDRESS+4)
-#define ADC_CH4_VOLTAGE                       (RAM_ADC_DMA_BASE_ADDRESS+6)
-#define ADC_CH5_VOLTAGE                       (RAM_ADC_DMA_BASE_ADDRESS+8)
-#define ADC_CH6_VOLTAGE                       (RAM_ADC_DMA_BASE_ADDRESS+10)
-#define ADC_CH7_VOLTAGE                       (RAM_ADC_DMA_BASE_ADDRESS+12)
-#define ADC_CH8_VOLTAGE                       (RAM_ADC_DMA_BASE_ADDRESS+14)
-#define ADC_CH9_VOLTAGE                       (RAM_ADC_DMA_BASE_ADDRESS+16)
-#define ADC_CH10_VOLTAGE                      (RAM_ADC_DMA_BASE_ADDRESS+18)
+#define DOWN_LEFT_MIDDLE_VOLTAGE              RAM_ADC_DMA_BASE_ADDRESS
+#define DOWN_LEFT_REAR_VOLTAGE                (RAM_ADC_DMA_BASE_ADDRESS+2)
+#define DOWN_ANALOG_VOLTAGE                   (RAM_ADC_DMA_BASE_ADDRESS+4)
+#define DOWN_LEFT_FRONT_VOLTAGE               (RAM_ADC_DMA_BASE_ADDRESS+6)
+#define DOWN_RIGHT_REAR_VOLTAGE               (RAM_ADC_DMA_BASE_ADDRESS+8)
+#define DOWN_RIGHT_MIDDLE_VOLTAGE             (RAM_ADC_DMA_BASE_ADDRESS+10)
+#define DOWN_RIGHT_FRONT_VOLTAGE              (RAM_ADC_DMA_BASE_ADDRESS+12)
+#define FRONT_LEFT_VOLTAGE                    (RAM_ADC_DMA_BASE_ADDRESS+14)
+#define FRONT_RIGHT_VOLTAGE                   (RAM_ADC_DMA_BASE_ADDRESS+16)
+#define FRONT_ANALOG_VOLTAGE                  (RAM_ADC_DMA_BASE_ADDRESS+18)
 #define ADC_TEMPERATURE                       (RAM_ADC_DMA_BASE_ADDRESS+20)
+#define ADC_VREFINT                           (RAM_ADC_DMA_BASE_ADDRESS+22)
 
 #endif
 
diff --git a/include/gpio.h b/include/gpio.h
index c48afc9bc87b66e29e3300b38031afad1f25a22c..59f9bca13cba480d8dc7de377d12be32c69b9cb1 100644
--- a/include/gpio.h
+++ b/include/gpio.h
@@ -3,7 +3,7 @@
 
 #include "stm32f1xx_hal.h"
 
-typedef enum {FRONT_LEFT,FRONT_RIGHT,DOWN_LEFT_FRONT,DOWN_LEFT_MIDDLE,DOWN_LEFT_REAR,DOWN_RIGHT_FRONT,DOWN_RIGHT_MIDDLE,DOWN_RIGHT_REAR} exp_led_t;
+typedef enum {FRONT_LEFT,FRONT_RIGHT,DOWN_LEFT_FRONT,DOWN_LEFT_MIDDLE,DOWN_LEFT_REAR,DOWN_RIGHT_FRONT,DOWN_RIGHT_MIDDLE,DOWN_RIGHT_REAR,FRONT_ANALOG,DOWN_ANALOG} exp_led_t;
 
 void gpio_init(void);
 /* output functions */
diff --git a/include/ir_feet_conf.h b/include/ir_feet_conf.h
index 98d7c3a459b2b5fae15d2eb4d24641b288da43e0..6e0564f67c77720ef072334188461f1a8b8a7c59 100644
--- a/include/ir_feet_conf.h
+++ b/include/ir_feet_conf.h
@@ -12,7 +12,7 @@
 
 #define DEFAULT_DEVICE_MODEL            0x000c
 #define DEFAULT_FIRMWARE_VERSION        0x01
-#define DEFAULT_DEVICE_ID               0x03
+#define DEFAULT_DEVICE_ID               0x01
 #define DEFAULT_BAUDRATE                0x10
 #define DEFAULT_RETURN_DELAY            0x00
 #define DEFAULT_RETURN_LEVEL            0x02
diff --git a/linker_script/ir_feet.ld b/linker_script/ir_feet.ld
index 974e77631fa282508f467532e085a6631df5fc4c..f6407c3a5676a80341a907ef6cadf8d72fb4c17c 100755
--- a/linker_script/ir_feet.ld
+++ b/linker_script/ir_feet.ld
@@ -9,7 +9,7 @@
 ENTRY(Reset_Handler)
 
 /* Highest address of the user mode stack */
-_estack = 0x20004FFF;    /* end of RAM */
+_estack = 0x20004BFF;    /* end of RAM */
 
 /* Generate a link error if heap and stack don't fit into RAM */
 _Min_Heap_Size = 0;      /* required amount of heap  */
@@ -20,7 +20,7 @@ MEMORY
 {
 ISR (rx)       : ORIGIN = 0x08000000, LENGTH = 1K
 EEPROM (rw)    : ORIGIN = 0x08000400, LENGTH = 2K
-FLASH (rx)     : ORIGIN = 0x08000C00, LENGTH = 125K
+FLASH (rx)     : ORIGIN = 0x08000C00, LENGTH = 61K
 RAM (xrw)      : ORIGIN = 0x20000000, LENGTH = 20K
 }
 
diff --git a/src/adc_dma.c b/src/adc_dma.c
index c8bd0f85670cb10d783e94a13a9b3bc3048ff67d..ed82d8b1cde44d6213afb2111e7cc18127ad167b 100755
--- a/src/adc_dma.c
+++ b/src/adc_dma.c
@@ -1,34 +1,60 @@
 #include "adc_dma.h"
 #include "adc_dma_registers.h"
 #include "ram.h"
- 
-#define ADC1_CH1                  ADC_CHANNEL_4
-#define ADC1_CH2                  ADC_CHANNEL_6
-#define ADC1_CH3                  ADC_CHANNEL_TEMPSENSOR
 
-#define ADC2_CH1                  ADC_CHANNEL_5
-#define ADC2_CH2                  ADC_CHANNEL_7
-#define ADC2_CH3                  ADC_CHANNEL_8
-
-#define ADC1_CH1_PIN              GPIO_PIN_4
+#define ADC1_CH1                  ADC_CHANNEL_0
+#define ADC1_CH2                  ADC_CHANNEL_2
+#define ADC1_CH3                  ADC_CHANNEL_4
+#define ADC1_CH4                  ADC_CHANNEL_6
+#define ADC1_CH5                  ADC_CHANNEL_TEMPSENSOR
+#define ADC1_CH6                  ADC_CHANNEL_VREFINT
+
+#define ADC2_CH1                  ADC_CHANNEL_1
+#define ADC2_CH2                  ADC_CHANNEL_3
+#define ADC2_CH3                  ADC_CHANNEL_5
+#define ADC2_CH4                  ADC_CHANNEL_7
+#define ADC2_CH5                  ADC_CHANNEL_8
+#define ADC2_CH6                  ADC_CHANNEL_9
+
+#define ADC1_CH1_PIN              GPIO_PIN_0
 #define ADC1_CH1_PORT             GPIOA
 #define ADC1_CH1_ENABLE_PORT_CLK  __GPIOA_CLK_ENABLE()
 
-#define ADC1_CH2_PIN              GPIO_PIN_6
+#define ADC1_CH2_PIN              GPIO_PIN_2
 #define ADC1_CH2_PORT             GPIOA
 #define ADC1_CH2_ENABLE_PORT_CLK  __GPIOA_CLK_ENABLE()
 
-#define ADC2_CH1_PIN              GPIO_PIN_5
+#define ADC1_CH3_PIN              GPIO_PIN_4
+#define ADC1_CH3_PORT             GPIOA
+#define ADC1_CH3_ENABLE_PORT_CLK  __GPIOA_CLK_ENABLE()
+
+#define ADC1_CH4_PIN              GPIO_PIN_6
+#define ADC1_CH4_PORT             GPIOA
+#define ADC1_CH4_ENABLE_PORT_CLK  __GPIOA_CLK_ENABLE()
+
+#define ADC2_CH1_PIN              GPIO_PIN_1
 #define ADC2_CH1_PORT             GPIOA
 #define ADC2_CH1_ENABLE_PORT_CLK  __GPIOA_CLK_ENABLE()
 
-#define ADC2_CH2_PIN              GPIO_PIN_7
+#define ADC2_CH2_PIN              GPIO_PIN_3
 #define ADC2_CH2_PORT             GPIOA
 #define ADC2_CH2_ENABLE_PORT_CLK  __GPIOA_CLK_ENABLE()
 
-#define ADC2_CH3_PIN              GPIO_PIN_0
-#define ADC2_CH3_PORT             GPIOB
-#define ADC2_CH3_ENABLE_PORT_CLK  __GPIOB_CLK_ENABLE()
+#define ADC2_CH3_PIN              GPIO_PIN_5
+#define ADC2_CH3_PORT             GPIOA
+#define ADC2_CH3_ENABLE_PORT_CLK  __GPIOA_CLK_ENABLE()
+
+#define ADC2_CH4_PIN              GPIO_PIN_7
+#define ADC2_CH4_PORT             GPIOA
+#define ADC2_CH4_ENABLE_PORT_CLK  __GPIOA_CLK_ENABLE()
+
+#define ADC2_CH5_PIN              GPIO_PIN_0
+#define ADC2_CH5_PORT             GPIOB
+#define ADC2_CH5_ENABLE_PORT_CLK  __GPIOB_CLK_ENABLE()
+
+#define ADC2_CH6_PIN              GPIO_PIN_1
+#define ADC2_CH6_PORT             GPIOB
+#define ADC2_CH6_ENABLE_PORT_CLK  __GPIOB_CLK_ENABLE()
 
 #define ADC1_ENABLE_CLK           __ADC1_CLK_ENABLE()
 #define ADC2_ENABLE_CLK           __ADC2_CLK_ENABLE()
@@ -44,7 +70,6 @@
 #define TEMP_INV_SLOPE            232.558// ADC slope in degree/mV
 // general voltage conversion
 #define VOLTAGE_DELTA             0.805664
-#define MOTOR_VOLTAGE_GAIN        16.0
 
 // private variables
 ADC_HandleTypeDef hadc1;
@@ -52,8 +77,9 @@ ADC_HandleTypeDef hadc2;
 DMA_HandleTypeDef hdma_adc1; 
 
 float adc_temperature;
-float adc_voltages[4];
-uint32_t adc_data[3];// temporal buffer to store ADC data before conversion
+float adc_vref;
+float adc_voltages[10];
+uint32_t adc_data[6];// temporal buffer to store ADC data before conversion
 
 /* memory module */
 TMemModule adc_mem_module;
@@ -68,37 +94,72 @@ void adc_convert_temperature(uint16_t value)
   ram_data[ADC_TEMPERATURE+1]=ram_value/256;
 }
 
+void adc_convert_vref(uint16_t value)
+{
+  uint16_t ram_value;
+
+  adc_vref=((float)value)*VOLTAGE_DELTA;
+  ram_value=(uint16_t)adc_vref;
+  ram_data[ADC_VREFINT]=ram_value%256;
+  ram_data[ADC_VREFINT+1]=ram_value/256;
+}
+
 void adc_convert_voltage(adc_dma_ch_t channel_id,uint16_t value)
 {
   uint16_t ram_value;
 
   switch(channel_id)
   {
-    case ADC_CH1: adc_voltages[0]=((float)value)*VOLTAGE_DELTA/adc_gains[0];
-                  ram_value=(uint16_t)adc_voltages[0];
-                  ram_data[ADC_CH1_VOLTAGE]=ram_value%256;
-                  ram_data[ADC_CH1_VOLTAGE+1]=ram_value/256;
-                  break;
-    case ADC_CH2: adc_voltages[1]=((float)value)*VOLTAGE_DELTA/adc_gains[1];
-                  ram_value=(uint16_t)adc_voltages[1];
-                  ram_data[ADC_CH2_VOLTAGE]=ram_value%256;
-                  ram_data[ADC_CH2_VOLTAGE+1]=ram_value/256;
-                  break;
-    case ADC_CH3: adc_voltages[2]=((float)value)*VOLTAGE_DELTA/adc_gains[2];
-                  ram_value=(uint16_t)adc_voltages[2];
-                  ram_data[ADC_CH3_VOLTAGE]=ram_value%256;
-                  ram_data[ADC_CH3_VOLTAGE+1]=ram_value/256;
-                  break;
-    case ADC_CH4: adc_voltages[3]=((float)value)*VOLTAGE_DELTA/adc_gains[3];
-                  ram_value=(uint16_t)adc_voltages[3];
-                  ram_data[ADC_CH4_VOLTAGE]=ram_value%256;
-                  ram_data[ADC_CH4_VOLTAGE+1]=ram_value/256;
-                  break;
-    case ADC_CH6: adc_motor_voltage=((float)value)*VOLTAGE_DELTA*MOTOR_VOLTAGE_GAIN;
-                  ram_value=(uint16_t)adc_motor_voltage;
-                  ram_data[ADC_MOTOR_VOLTAGE]=ram_value%256;
-                  ram_data[ADC_MOTOR_VOLTAGE+1]=ram_value/256;
-                  break;
+    case DOWN_LEFT_MIDDLE_CH: adc_voltages[0]=((float)value)*VOLTAGE_DELTA;
+                              ram_value=(uint16_t)adc_voltages[0];
+                              ram_data[DOWN_LEFT_MIDDLE_VOLTAGE]=ram_value%256;
+                              ram_data[DOWN_LEFT_MIDDLE_VOLTAGE+1]=ram_value/256;
+                              break;
+    case DOWN_LEFT_REAR_CH: adc_voltages[1]=((float)value)*VOLTAGE_DELTA;
+                            ram_value=(uint16_t)adc_voltages[1];
+                            ram_data[DOWN_LEFT_REAR_VOLTAGE]=ram_value%256;
+                            ram_data[DOWN_LEFT_REAR_VOLTAGE+1]=ram_value/256;
+                            break;
+    case DOWN_ANALOG_CH: adc_voltages[2]=((float)value)*VOLTAGE_DELTA;
+                         ram_value=(uint16_t)adc_voltages[2];
+                         ram_data[DOWN_ANALOG_VOLTAGE]=ram_value%256;
+                         ram_data[DOWN_ANALOG_VOLTAGE+1]=ram_value/256;
+                         break;
+    case DOWN_LEFT_FRONT_CH: adc_voltages[3]=((float)value)*VOLTAGE_DELTA;
+                             ram_value=(uint16_t)adc_voltages[3];
+                             ram_data[DOWN_LEFT_FRONT_VOLTAGE]=ram_value%256;
+                             ram_data[DOWN_LEFT_FRONT_VOLTAGE+1]=ram_value/256;
+                             break;
+    case DOWN_RIGHT_REAR_CH: adc_voltages[4]=((float)value)*VOLTAGE_DELTA;
+                             ram_value=(uint16_t)adc_voltages[0];
+                             ram_data[DOWN_RIGHT_REAR_VOLTAGE]=ram_value%256;
+                             ram_data[DOWN_RIGHT_REAR_VOLTAGE+1]=ram_value/256;
+                             break;
+    case DOWN_RIGHT_MIDDLE_CH: adc_voltages[5]=((float)value)*VOLTAGE_DELTA;
+                               ram_value=(uint16_t)adc_voltages[0];
+                               ram_data[DOWN_RIGHT_MIDDLE_VOLTAGE]=ram_value%256;
+                               ram_data[DOWN_RIGHT_MIDDLE_VOLTAGE+1]=ram_value/256;
+                               break;
+    case DOWN_RIGHT_FRONT_CH: adc_voltages[6]=((float)value)*VOLTAGE_DELTA;
+                              ram_value=(uint16_t)adc_voltages[0];
+                              ram_data[DOWN_RIGHT_FRONT_VOLTAGE]=ram_value%256;
+                              ram_data[DOWN_RIGHT_FRONT_VOLTAGE+1]=ram_value/256;
+                              break;
+    case FRONT_LEFT_CH: adc_voltages[7]=((float)value)*VOLTAGE_DELTA;
+                        ram_value=(uint16_t)adc_voltages[0];
+                        ram_data[FRONT_LEFT_VOLTAGE]=ram_value%256;
+                        ram_data[FRONT_LEFT_VOLTAGE+1]=ram_value/256;
+                        break;
+    case FRONT_RIGHT_CH: adc_voltages[8]=((float)value)*VOLTAGE_DELTA;
+                         ram_value=(uint16_t)adc_voltages[0];
+                         ram_data[FRONT_RIGHT_VOLTAGE]=ram_value%256;
+                         ram_data[FRONT_RIGHT_VOLTAGE+1]=ram_value/256;
+                         break;
+    case FRONT_ANALOG_CH: adc_voltages[9]=((float)value)*VOLTAGE_DELTA;
+                          ram_value=(uint16_t)adc_voltages[0];
+                          ram_data[FRONT_ANALOG_VOLTAGE]=ram_value%256;
+                          ram_data[FRONT_ANALOG_VOLTAGE+1]=ram_value/256;
+                          break;
     default: break;
   }
 }
@@ -124,14 +185,19 @@ void ADC_DMA_IRQHandler(void)
   if(__HAL_DMA_GET_FLAG(&hdma_adc1, __HAL_DMA_GET_TC_FLAG_INDEX(&hdma_adc1)) != RESET)
     if(__HAL_DMA_GET_IT_SOURCE(&hdma_adc1, DMA_IT_TC) != RESET)
     {
-      __HAL_DMA_CLEAR_FLAG(&hdma_adc1, __HAL_DMA_GET_TC_FLAG_INDEX(&hdma_adc1));
       HAL_ADC_Stop(&hadc2);
-      adc_convert_voltage(ADC_CH1,adc_data[0]&0x0000FFFF);
-      adc_convert_voltage(ADC_CH2,(adc_data[0]>>16)&0x0000FFFF);
-      adc_convert_voltage(ADC_CH3,adc_data[1]&0x0000FFFF);
-      adc_convert_voltage(ADC_CH4,(adc_data[1]>>16)&0x0000FFFF);
-      adc_convert_temperature(adc_data[2]&0x0000FFFF);
-      adc_convert_voltage(ADC_CH6,(adc_data[2]>>16)&0x0000FFFF);
+      adc_convert_voltage(DOWN_LEFT_MIDDLE_CH,adc_data[0]&0x0000FFFF);
+      adc_convert_voltage(DOWN_LEFT_REAR_CH,(adc_data[0]>>16)&0x0000FFFF);
+      adc_convert_voltage(DOWN_ANALOG_CH,adc_data[1]&0x0000FFFF);
+      adc_convert_voltage(DOWN_LEFT_FRONT_CH,(adc_data[1]>>16)&0x0000FFFF);
+      adc_convert_voltage(DOWN_RIGHT_REAR_CH,adc_data[2]&0x0000FFFF);
+      adc_convert_voltage(DOWN_RIGHT_MIDDLE_CH,(adc_data[2]>>16)&0x0000FFFF);
+      adc_convert_voltage(DOWN_RIGHT_FRONT_CH,adc_data[3]&0x0000FFFF);
+      adc_convert_voltage(FRONT_LEFT_CH,(adc_data[3]>>16)&0x0000FFFF);
+      adc_convert_temperature(adc_data[4]&0x0000FFFF);
+      adc_convert_voltage(FRONT_RIGHT_CH,(adc_data[4]>>16)&0x0000FFFF);
+      adc_convert_vref(adc_data[5]&0x0000FFFF);
+      adc_convert_voltage(FRONT_ANALOG_CH,(adc_data[5]>>16)&0x0000FFFF);
     }
 }
 
@@ -145,9 +211,14 @@ uint8_t adc_init(TMemory *memory)
   /* enable clocks */
   ADC1_CH1_ENABLE_PORT_CLK;
   ADC1_CH2_ENABLE_PORT_CLK;
+  ADC1_CH3_ENABLE_PORT_CLK;
+  ADC1_CH4_ENABLE_PORT_CLK;
   ADC2_CH1_ENABLE_PORT_CLK;
   ADC2_CH2_ENABLE_PORT_CLK;
   ADC2_CH3_ENABLE_PORT_CLK;
+  ADC2_CH4_ENABLE_PORT_CLK;
+  ADC2_CH5_ENABLE_PORT_CLK;
+  ADC2_CH6_ENABLE_PORT_CLK;
   
   ADC1_ENABLE_CLK;
   ADC2_ENABLE_CLK;
@@ -161,6 +232,12 @@ uint8_t adc_init(TMemory *memory)
   GPIO_InitStruct.Pin = ADC1_CH2_PIN;
   HAL_GPIO_Init(ADC1_CH2_PORT, &GPIO_InitStruct);
 
+  GPIO_InitStruct.Pin = ADC1_CH3_PIN;
+  HAL_GPIO_Init(ADC1_CH3_PORT, &GPIO_InitStruct);
+
+  GPIO_InitStruct.Pin = ADC1_CH4_PIN;
+  HAL_GPIO_Init(ADC1_CH4_PORT, &GPIO_InitStruct);
+
   GPIO_InitStruct.Pin = ADC2_CH1_PIN;
   HAL_GPIO_Init(ADC2_CH1_PORT, &GPIO_InitStruct);
 
@@ -170,6 +247,15 @@ uint8_t adc_init(TMemory *memory)
   GPIO_InitStruct.Pin = ADC2_CH3_PIN;
   HAL_GPIO_Init(ADC2_CH3_PORT, &GPIO_InitStruct);
 
+  GPIO_InitStruct.Pin = ADC2_CH4_PIN;
+  HAL_GPIO_Init(ADC2_CH4_PORT, &GPIO_InitStruct);
+
+  GPIO_InitStruct.Pin = ADC2_CH5_PIN;
+  HAL_GPIO_Init(ADC2_CH5_PORT, &GPIO_InitStruct);
+
+  GPIO_InitStruct.Pin = ADC2_CH6_PIN;
+  HAL_GPIO_Init(ADC2_CH6_PORT, &GPIO_InitStruct);
+
   ADC_ENABLE_DMA_CLK;
 
   //initialize DMA 
@@ -195,7 +281,7 @@ uint8_t adc_init(TMemory *memory)
   hadc1.Init.DiscontinuousConvMode = DISABLE;
   hadc1.Init.ExternalTrigConv = ADC_SOFTWARE_START;
   hadc1.Init.DataAlign = ADC_DATAALIGN_RIGHT;
-  hadc1.Init.NbrOfConversion = 3;
+  hadc1.Init.NbrOfConversion = 6;
   HAL_ADC_Init(&hadc1);
 
   multimode.Mode = ADC_DUALMODE_REGSIMULT;
@@ -215,6 +301,18 @@ uint8_t adc_init(TMemory *memory)
   sConfig.Rank = 3;
   HAL_ADC_ConfigChannel(&hadc1, &sConfig);
 
+  sConfig.Channel = ADC1_CH4;
+  sConfig.Rank = 4;
+  HAL_ADC_ConfigChannel(&hadc1, &sConfig);
+
+  sConfig.Channel = ADC1_CH5;
+  sConfig.Rank = 5;
+  HAL_ADC_ConfigChannel(&hadc1, &sConfig);
+
+  sConfig.Channel = ADC1_CH6;
+  sConfig.Rank = 6;
+  HAL_ADC_ConfigChannel(&hadc1, &sConfig);
+
   // configure ADC2 
   hadc2.Instance = ADC2;
   hadc2.Init.ScanConvMode = ADC_SCAN_ENABLE;
@@ -222,7 +320,7 @@ uint8_t adc_init(TMemory *memory)
   hadc2.Init.DiscontinuousConvMode = DISABLE;
   hadc2.Init.ExternalTrigConv = ADC_SOFTWARE_START;
   hadc2.Init.DataAlign = ADC_DATAALIGN_RIGHT;
-  hadc2.Init.NbrOfConversion = 3;
+  hadc2.Init.NbrOfConversion = 6;
   HAL_ADC_Init(&hadc2);
 
   multimode.Mode = ADC_DUALMODE_REGSIMULT;
@@ -242,22 +340,28 @@ uint8_t adc_init(TMemory *memory)
   sConfig.Rank = 3;
   HAL_ADC_ConfigChannel(&hadc2, &sConfig);
 
+  sConfig.Channel = ADC2_CH4;
+  sConfig.Rank = 4;
+  HAL_ADC_ConfigChannel(&hadc2, &sConfig);
+
+  sConfig.Channel = ADC2_CH5;
+  sConfig.Rank = 5;
+  HAL_ADC_ConfigChannel(&hadc2, &sConfig);
+
+  sConfig.Channel = ADC2_CH6;
+  sConfig.Rank = 6;
+  HAL_ADC_ConfigChannel(&hadc2, &sConfig);
+
   /* calibrate ADC1 */
   HAL_ADCEx_Calibration_Start(&hadc1);
   HAL_ADCEx_Calibration_Start(&hadc2);
 
-  /* initialize the digital potentiometer modules */
-  digital_pot_init(&pot1,0x58,POT_20k);
-  digital_pot_init(&pot2,0x5A,POT_20k);
-
   /* initialize memory module */
   mem_module_init(&adc_mem_module);
   adc_mem_module.write_cmd=adc_write_cmd;
   adc_mem_module.read_cmd=adc_read_cmd;
   if(!mem_module_add_ram_segment(&adc_mem_module,RAM_ADC_DMA_BASE_ADDRESS,RAM_ADC_DMA_LENGTH))
     return 0x00;
-  if(!mem_module_add_eeprom_segment(&adc_mem_module,EEPROM_ADC_DMA_BASE_ADDRESS,EEPROM_ADC_DMA_LENGTH))
-    return 0x00;
   if(!mem_add_module(memory,&adc_mem_module))
     return 0x00;
 
@@ -267,17 +371,23 @@ uint8_t adc_init(TMemory *memory)
 void adc_start_conversion(void)
 {
   HAL_ADC_Start(&hadc2);
-  HAL_ADCEx_MultiModeStart_DMA(&hadc1,adc_data,3);
+  HAL_ADCEx_MultiModeStart_DMA(&hadc1,adc_data,6);
 }
 
 float adc_get_voltage(adc_dma_ch_t channel_id)
 {
   switch(channel_id)
   {
-    case ADC_CH1: return adc_voltages[0];
-    case ADC_CH2: return adc_voltages[1];
-    case ADC_CH3: return adc_voltages[2];
-    case ADC_CH4: return adc_voltages[3];
+    case DOWN_LEFT_MIDDLE_CH: return adc_voltages[0];
+    case DOWN_LEFT_REAR_CH: return adc_voltages[1];
+    case DOWN_ANALOG_CH: return adc_voltages[2];
+    case DOWN_LEFT_FRONT_CH: return adc_voltages[3];
+    case DOWN_RIGHT_REAR_CH: return adc_voltages[4];
+    case DOWN_RIGHT_MIDDLE_CH: return adc_voltages[5];
+    case DOWN_RIGHT_FRONT_CH: return adc_voltages[6];
+    case FRONT_LEFT_CH: return adc_voltages[7];
+    case FRONT_RIGHT_CH: return adc_voltages[8];
+    case FRONT_ANALOG_CH: return adc_voltages[9];
     default: return 0.0;
   }
 }
@@ -286,3 +396,8 @@ inline float adc_get_temperature(void)
 {
   return adc_temperature;
 }
+
+inline float adc_get_vref(void)
+{
+  return adc_vref;
+}
diff --git a/src/gpio.c b/src/gpio.c
index cb982b414114803f03e28c2bdb07c36312a592ca..784ec57917b6239bfffaad8b83fcd0f0a6618862 100644
--- a/src/gpio.c
+++ b/src/gpio.c
@@ -1,372 +1,143 @@
 #include "gpio.h"
 
-#define GPIO_EXP_ADDRESS                  0x40
-#define GPIO_EXP_NUM_REG                  22
-
-#define INTA_GPIO_CLK                     __GPIOC_CLK_ENABLE()
-#define INTA_PIN                          GPIO_PIN_14
-#define INTA_GPIO_PORT                    GPIOC
-
-#define INTA_IRQn                         EXTI15_10_IRQn
-#define INTA_IRQnHandler                  EXTI15_10_IRQHandler
-
-#define POLOLU_ENABLE_PORT                EXP_PORTA
-#define POLOLU_ENABLE_PIN                 EXP_PIN3                       
-
-#define POLOLU_FAULT1_PORT                EXP_PORTA
-#define POLOLU_FAULT1_PIN                 EXP_PIN2     
-
-#define POLOLU_FAULT2_PORT                EXP_PORTA
-#define POLOLU_FAULT2_PIN                 EXP_PIN1     
-
-#define LIMIT_SW_FWD_CONF_PORT            EXP_PORTA
-#define LIMIT_SW_FWD_CONF_PIN             EXP_PIN8
-
-#define LIMIT_SW_RVS_CONF_PORT            EXP_PORTA
-#define LIMIT_SW_RVS_CONF_PIN             EXP_PIN6
-
-#define LIMIT_SW_HOME_CONF_PORT           EXP_PORTA
-#define LIMIT_SW_HOME_CONF_PIN            EXP_PIN7
-
-#define LED0_PORT                         EXP_PORTB
-#define LED0_PIN                          EXP_PIN5
-
-#define LED1_PORT                         EXP_PORTB
-#define LED1_PIN                          EXP_PIN6
-
-#define LED2_PORT                         EXP_PORTB
-#define LED2_PIN                          EXP_PIN7
-
-#define LED3_PORT                         EXP_PORTB
-#define LED3_PIN                          EXP_PIN8
-
-/* sync module data */
-#define SYNC_IN_PIN                       GPIO_PIN_12
-
-extern void sync_interrupt(void);
-
-/* GPIO expander registers */
-typedef enum {EXP_IODIRA=0x00,
-              EXP_IODIRB=0x01,
-              EXP_IPOLA=0x02,
-              EXP_IPOLB=0x03,
-              EXP_GPINTENA=0x04,
-              EXP_GPINTENB=0x05,
-              EXP_DEFVALA=0x06,
-              EXP_DEFVALB=0x07,
-              EXP_INTCONA=0x08,
-              EXP_INTCONB=0x09,
-              EXP_IOCONA=0x0A,
-              EXP_IOCONB=0x0B,
-              EXP_GPPUA=0x0C,
-              EXP_GPPUB=0x0D,
-              EXP_INTFA=0x0E,
-              EXP_INTFB=0x0F,
-              EXP_INTCAPA=0x10,
-              EXP_INTCAPB=0x11,
-              EXP_GPIOA=0x12,
-              EXP_GPIOB=0x13,
-              EXP_OLATA=0x14,
-              EXP_OLATB=0x15} t_gpio_reg;
-
-typedef enum {EXP_PORTA=0,EXP_PORTB=1} exp_port_t;
-
-typedef enum {EXP_PIN1=0x01,EXP_PIN2=0x02,EXP_PIN3=0x04,EXP_PIN4=0x08,EXP_PIN5=0x10,EXP_PIN6=0x20,EXP_PIN7=0x40,EXP_PIN8=0x80} exp_pin_t;
-
-typedef enum {EXP_IN=1,EXP_OUT=0} exp_dir_t;
-
-typedef enum {EXP_INT_CMP_LAST=0,EXP_INT_CMP_DEFAULT=1} exp_int_op_t;
-
-typedef enum {EXP_NO_EDGE,EXP_RISING_EDGE,EXP_FALLING_EDGE,EXP_BOTH_EDGES} exp_int_edge_t;
+#define DOWN_RIGHT_FRONT_GPIO_CLK                __GPIOB_CLK_ENABLE()
+#define DOWN_RIGHT_FRONT_PIN                     GPIO_PIN_8
+#define DOWN_RIGHT_FRONT_GPIO_PORT               GPIOB
+
+#define DOWN_RIGHT_REAR_GPIO_CLK                 __GPIOB_CLK_ENABLE()
+#define DOWN_RIGHT_REAR_PIN                      GPIO_PIN_9
+#define DOWN_RIGHT_REAR_GPIO_PORT                GPIOB
+
+#define DOWN_LEFT_REAR_GPIO_CLK                  __GPIOB_CLK_ENABLE()
+#define DOWN_LEFT_REAR_PIN                       GPIO_PIN_7
+#define DOWN_LEFT_REAR_GPIO_PORT                 GPIOB
+
+#define DOWN_LEFT_MIDDLE_GPIO_CLK                __GPIOB_CLK_ENABLE()
+#define DOWN_LEFT_MIDDLE_PIN                     GPIO_PIN_11
+#define DOWN_LEFT_MIDDLE_GPIO_PORT               GPIOB
+
+#define DOWN_RIGHT_MIDDLE_GPIO_CLK               __GPIOB_CLK_ENABLE()
+#define DOWN_RIGHT_MIDDLE_PIN                    GPIO_PIN_2
+#define DOWN_RIGHT_MIDDLE_GPIO_PORT              GPIOB
+ 
+#define DOWN_LEFT_FRONT_GPIO_CLK                 __GPIOB_CLK_ENABLE()
+#define DOWN_LEFT_FRONT_PIN                      GPIO_PIN_10
+#define DOWN_LEFT_FRONT_GPIO_PORT                GPIOB
+
+#define FRONT_LEFT_GPIO_CLK                      __GPIOC_CLK_ENABLE()
+#define FRONT_LEFT_PIN                           GPIO_PIN_15
+#define FRONT_LEFT_GPIO_PORT                     GPIOC
+
+#define FRONT_RIGHT_GPIO_CLK                     __GPIOB_CLK_ENABLE()
+#define FRONT_RIGHT_PIN                          GPIO_PIN_14
+#define FRONT_RIGHT_GPIO_PORT                    GPIOB
+
+#define DOWN_ANALOG_GPIO_CLK                     __GPIOB_CLK_ENABLE()
+#define DOWN_ANALOG_PIN                          GPIO_PIN_13
+#define DOWN_ANALOG_GPIO_PORT                    GPIOB
+
+#define FRONT_ANALOG_GPIO_CLK                    __GPIOB_CLK_ENABLE()
+#define FRONT_ANALOG_PIN                         GPIO_PIN_12
+#define FRONT_ANALOG_GPIO_PORT                   GPIOB
 
 // private variables
-unsigned char gpio_exp_registers[GPIO_EXP_NUM_REG];
-unsigned char gpio_pololu_status;
-unsigned char gpio_sw_status;
 
 // private functions
-void gpio_process_input_data(void)
-{
-  if(gpio_exp_registers[EXP_INTFA]&0x03)
-    gpio_pololu_status=gpio_exp_registers[EXP_INTCAPA]&0x03;
-  if(gpio_exp_registers[EXP_INTFB]&0x0F)
-    gpio_sw_status=gpio_exp_registers[EXP_INTCAPB]&0x0F;
-}
-
-void gpio_set_direction(exp_port_t port, exp_pin_t pin, exp_dir_t dir)
-{
-  unsigned char address;
-
-  if(port==EXP_PORTA)
-    address=EXP_IODIRA;
-  else
-    address=EXP_IODIRB;
-
-  if(dir==EXP_IN)
-    gpio_exp_registers[address]|=pin;
-  else
-    gpio_exp_registers[address]&=(~pin);
-}
-
-void gpio_set_polarity(exp_port_t port, exp_pin_t pin, unsigned char inv_pol)
-{
-  unsigned char address;
-
-  if(port==EXP_PORTA)
-    address=EXP_IPOLA;
-  else
-    address=EXP_IPOLB;
-
-  if(inv_pol)
-    gpio_exp_registers[address]|=pin;
-  else
-    gpio_exp_registers[address]&=(~pin);
-}
 
-void gpio_enable_int(exp_port_t port, exp_pin_t pin, unsigned char enable)
-{
-  unsigned char address;
-
-  if(port==EXP_PORTA)
-    address=EXP_GPINTENA;
-  else
-    address=EXP_GPINTENB;
-  if(enable)
-    gpio_exp_registers[address]|=pin;
-  else
-    gpio_exp_registers[address]&=(~pin);
-}
-
-void gpio_set_int_cmp_value(exp_port_t port, exp_pin_t pin, unsigned char value)
-{
-  unsigned char address;
-
-  if(port==EXP_PORTA)
-    address=EXP_DEFVALA;
-  else
-    address=EXP_DEFVALB;
-  if(value)
-    gpio_exp_registers[address]|=pin;
-  else
-    gpio_exp_registers[address]&=(~pin);
-}
-
-void gpio_set_int_operation(exp_port_t port, exp_pin_t pin, exp_int_op_t op)
-{
-  unsigned char address;
-
-  if(port==EXP_PORTA)
-    address=EXP_INTCONA;
-  else
-    address=EXP_INTCONB;
-  if(op==EXP_INT_CMP_DEFAULT)
-    gpio_exp_registers[address]|=pin;
-  else
-    gpio_exp_registers[address]&=(~pin);
-}
-
-void gpio_enable_pull_up(exp_port_t port, exp_pin_t pin,unsigned char enable)
-{
-  unsigned char address;
-
-  if(port==EXP_PORTA)
-    address=EXP_GPPUA;
-  else
-    address=EXP_GPPUB;
-  if(enable)
-    gpio_exp_registers[address]|=pin;
-  else
-    gpio_exp_registers[address]&=(~pin);
-}
-
-void gpio_config(unsigned char shared_int, unsigned char auto_inc_addr, unsigned char slew_rate, unsigned char int_open_drain, unsigned char int_pol)
-{
-  // change the configuration
-  if(shared_int)
-  {
-    gpio_exp_registers[EXP_IOCONA]|=0x40;
-    gpio_exp_registers[EXP_IOCONB]|=0x40;
-  }
-  else
-  {
-    gpio_exp_registers[EXP_IOCONA]&=0xBF;
-    gpio_exp_registers[EXP_IOCONB]&=0xBF;
-  }
-  if(auto_inc_addr)
-  {
-    gpio_exp_registers[EXP_IOCONA]&=0xDF;
-    gpio_exp_registers[EXP_IOCONB]&=0xDF;
-  }
-  else
-  {
-    gpio_exp_registers[EXP_IOCONA]|=0x20;
-    gpio_exp_registers[EXP_IOCONB]|=0x20;
-  }
-  if(slew_rate)
-  {
-    gpio_exp_registers[EXP_IOCONA]&=0xEF;
-    gpio_exp_registers[EXP_IOCONB]&=0xEF;
-  }
-  else
-  {
-    gpio_exp_registers[EXP_IOCONA]|=0x10;
-    gpio_exp_registers[EXP_IOCONB]|=0x10;
-  }
-  if(int_open_drain)
-  {
-    gpio_exp_registers[EXP_IOCONA]|=0x04;
-    gpio_exp_registers[EXP_IOCONB]|=0x04;
-  }
-  else
-  {
-    gpio_exp_registers[EXP_IOCONA]&=0xFB;
-    gpio_exp_registers[EXP_IOCONB]&=0xFB;
-  }
-  if(int_pol)
-  {
-    gpio_exp_registers[EXP_IOCONA]|=0x02;
-    gpio_exp_registers[EXP_IOCONB]|=0x02;
-  }
-  else
-  {
-    gpio_exp_registers[EXP_IOCONA]&=0xFD;
-    gpio_exp_registers[EXP_IOCONB]&=0xFD;
-  }
-}
-
-void gpio_config_pin(exp_port_t port, exp_pin_t pin,exp_dir_t dir, unsigned char inv_pol, unsigned char pull_up)
+/* public functions */
+void gpio_init(void)
 {
-  gpio_set_direction(port,pin,dir);
-  gpio_set_polarity(port,pin,inv_pol);
-  gpio_enable_pull_up(port,pin,pull_up);
-}
+  GPIO_InitTypeDef GPIO_InitStruct;
 
-void gpio_set_pin_value(exp_port_t port, exp_pin_t pin, unsigned char value)
-{
-  unsigned char address;
+  // configure the external interrutps
+  DOWN_RIGHT_FRONT_GPIO_CLK;
+  DOWN_RIGHT_MIDDLE_GPIO_CLK;
+  DOWN_RIGHT_REAR_GPIO_CLK;
+  DOWN_LEFT_FRONT_GPIO_CLK;
+  DOWN_LEFT_MIDDLE_GPIO_CLK;
+  DOWN_LEFT_REAR_GPIO_CLK;
+  FRONT_LEFT_GPIO_CLK;
+  FRONT_RIGHT_GPIO_CLK;
+  DOWN_ANALOG_GPIO_CLK;
+  FRONT_ANALOG_GPIO_CLK;
 
-  if(port==EXP_PORTA)
-    address=EXP_OLATA;
-  else
-    address=EXP_OLATB;
-  if(value)
-    gpio_exp_registers[address]|=pin;
-  else
-    gpio_exp_registers[address]&=(~pin);
-  while(i2c_queue_full());
-  i2c_write(GPIO_EXP_ADDRESS,address,1,&gpio_exp_registers[address]);
-}
+  /* configure GPIO's */
+  GPIO_InitStruct.Pin = DOWN_RIGHT_FRONT_PIN;
+  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
+  GPIO_InitStruct.Pull = GPIO_NOPULL;
+  HAL_GPIO_Init(DOWN_RIGHT_FRONT_GPIO_PORT, &GPIO_InitStruct);
 
-unsigned char gpio_get_pin_value(exp_port_t port, exp_pin_t pin)
-{
-  return 0x00;
-}
+  GPIO_InitStruct.Pin = DOWN_RIGHT_MIDDLE_PIN;
+  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
+  GPIO_InitStruct.Pull = GPIO_NOPULL;
+  HAL_GPIO_Init(DOWN_RIGHT_MIDDLE_GPIO_PORT, &GPIO_InitStruct);
 
-/* interrupt handlers */
-void INTA_IRQnHandler(void)
-{
-  if(__HAL_GPIO_EXTI_GET_IT(INTA_PIN) != RESET)
-  {
-    __HAL_GPIO_EXTI_CLEAR_IT(INTA_PIN);
-    /* read the flag and captured values of both ports */
-    i2c_read_cb(GPIO_EXP_ADDRESS,EXP_INTFA,4,&gpio_exp_registers[EXP_INTFA],gpio_process_input_data);
-  }
-  else if(__HAL_GPIO_EXTI_GET_IT(SYNC_IN_PIN) != RESET)
-  {
-    __HAL_GPIO_EXTI_CLEAR_IT(SYNC_IN_PIN);
-    sync_interrupt(); 
-  }
-}
+  GPIO_InitStruct.Pin = DOWN_RIGHT_REAR_PIN;
+  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
+  GPIO_InitStruct.Pull = GPIO_NOPULL;
+  HAL_GPIO_Init(DOWN_RIGHT_REAR_GPIO_PORT, &GPIO_InitStruct);
 
-/* public functions */
-void gpio_init(void)
-{
-  GPIO_InitTypeDef GPIO_InitStruct;
+  GPIO_InitStruct.Pin = DOWN_LEFT_FRONT_PIN;
+  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
+  GPIO_InitStruct.Pull = GPIO_NOPULL;
+  HAL_GPIO_Init(DOWN_LEFT_FRONT_GPIO_PORT, &GPIO_InitStruct);
 
-  // read all the registers
-  i2c_read(GPIO_EXP_ADDRESS,EXP_IODIRA,GPIO_EXP_NUM_REG,&gpio_exp_registers[0]);
-  while(!i2c_queue_empty());
-  // configure the GPIO expansion IC
-  gpio_config(0x01,// shared interrupts
-              0x01,// auto increment internal address
-              0x01,// enable slew rate control
-              0x00,// use push pull outputs for interrupts
-              0x01);// use active high interrupts
-  // configure PORTA
-  gpio_config_pin(EXP_PORTA,EXP_PIN1,EXP_IN,0x00,0x00);// Pololu FF2 signal
-  gpio_enable_int(EXP_PORTA,EXP_PIN1,0x01);
-  gpio_set_int_operation(EXP_PORTA,EXP_PIN1,EXP_INT_CMP_LAST);
-  gpio_config_pin(EXP_PORTA,EXP_PIN2,EXP_IN,0x00,0x00);// Pololu FF1 signal
-  gpio_enable_int(EXP_PORTA,EXP_PIN2,0x01);
-  gpio_set_int_operation(EXP_PORTA,EXP_PIN2,EXP_INT_CMP_LAST);
-  gpio_config_pin(EXP_PORTA,EXP_PIN3,EXP_OUT,0x00,0x00);// Pololu nRESET signal
-  gpio_config_pin(EXP_PORTA,EXP_PIN4,EXP_IN,0x00,0x00);//
-  gpio_config_pin(EXP_PORTA,EXP_PIN5,EXP_IN,0x00,0x00);//
-  gpio_config_pin(EXP_PORTA,EXP_PIN6,EXP_OUT,0x00,0x00);// Reverse switch control
-  gpio_config_pin(EXP_PORTA,EXP_PIN7,EXP_OUT,0x00,0x00);// Home switch control
-  gpio_config_pin(EXP_PORTA,EXP_PIN8,EXP_OUT,0x00,0x00);// Forward switch control
-  // Configure PORTB
-  gpio_config_pin(EXP_PORTB,EXP_PIN1,EXP_IN,0x00,0x00);// switch input0
-  gpio_enable_int(EXP_PORTB,EXP_PIN1,0x01);
-  gpio_set_int_operation(EXP_PORTB,EXP_PIN1,EXP_INT_CMP_LAST);
-  gpio_config_pin(EXP_PORTB,EXP_PIN2,EXP_IN,0x00,0x00);// switch input1
-  gpio_enable_int(EXP_PORTB,EXP_PIN2,0x01);
-  gpio_set_int_operation(EXP_PORTB,EXP_PIN2,EXP_INT_CMP_LAST);
-  gpio_config_pin(EXP_PORTB,EXP_PIN3,EXP_IN,0x00,0x00);// switch input2
-  gpio_enable_int(EXP_PORTB,EXP_PIN3,0x01);
-  gpio_set_int_operation(EXP_PORTB,EXP_PIN3,EXP_INT_CMP_LAST);
-  gpio_config_pin(EXP_PORTB,EXP_PIN4,EXP_IN,0x00,0x00);// switch input3
-  gpio_enable_int(EXP_PORTB,EXP_PIN4,0x01);
-  gpio_set_int_operation(EXP_PORTB,EXP_PIN4,EXP_INT_CMP_LAST);
-  gpio_config_pin(EXP_PORTB,EXP_PIN5,EXP_OUT,0x00,0x00);// LED0
-  gpio_config_pin(EXP_PORTB,EXP_PIN6,EXP_OUT,0x00,0x00);// LED1
-  gpio_config_pin(EXP_PORTB,EXP_PIN7,EXP_OUT,0x00,0x00);// LED2
-  gpio_config_pin(EXP_PORTB,EXP_PIN8,EXP_OUT,0x00,0x00);// LED3
+  GPIO_InitStruct.Pin = DOWN_LEFT_MIDDLE_PIN;
+  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
+  GPIO_InitStruct.Pull = GPIO_NOPULL;
+  HAL_GPIO_Init(DOWN_LEFT_MIDDLE_GPIO_PORT, &GPIO_InitStruct);
 
-  // write the configuration of all registers
-  i2c_write(GPIO_EXP_ADDRESS,EXP_IODIRA,GPIO_EXP_NUM_REG,&gpio_exp_registers[0]);
-  while(!i2c_queue_empty());
+  GPIO_InitStruct.Pin = DOWN_LEFT_REAR_PIN;
+  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
+  GPIO_InitStruct.Pull = GPIO_NOPULL;
+  HAL_GPIO_Init(DOWN_LEFT_REAR_GPIO_PORT, &GPIO_InitStruct);
 
-  // initialize internal variables
-  gpio_pololu_status=gpio_exp_registers[EXP_INTCAPA]&0x03;
-  gpio_sw_status=gpio_exp_registers[EXP_INTCAPB]&0x0F;
+  GPIO_InitStruct.Pin = FRONT_LEFT_PIN;
+  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
+  GPIO_InitStruct.Pull = GPIO_NOPULL;
+  HAL_GPIO_Init(FRONT_LEFT_GPIO_PORT, &GPIO_InitStruct);
 
-  // configure the external interrutps
-  INTA_GPIO_CLK;
+  GPIO_InitStruct.Pin = FRONT_RIGHT_PIN;
+  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
+  GPIO_InitStruct.Pull = GPIO_NOPULL;
+  HAL_GPIO_Init(FRONT_RIGHT_GPIO_PORT, &GPIO_InitStruct);
 
-  /* configure GPIO's */
-  GPIO_InitStruct.Pin = INTA_PIN;
-  GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING;
+  GPIO_InitStruct.Pin = DOWN_ANALOG_PIN;
+  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
   GPIO_InitStruct.Pull = GPIO_NOPULL;
-  HAL_GPIO_Init(INTA_GPIO_PORT, &GPIO_InitStruct);
+  HAL_GPIO_Init(DOWN_ANALOG_GPIO_PORT, &GPIO_InitStruct);
 
-  HAL_NVIC_SetPriority(INTA_IRQn, 3, 3);
-  HAL_NVIC_EnableIRQ(INTA_IRQn);
+  GPIO_InitStruct.Pin = FRONT_ANALOG_PIN;
+  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
+  GPIO_InitStruct.Pull = GPIO_NOPULL;
+  HAL_GPIO_Init(FRONT_ANALOG_GPIO_PORT, &GPIO_InitStruct);
 }
 
 /* output functions */
-void gpio_enable_pololu(void)
-{
-  gpio_set_pin_value(POLOLU_ENABLE_PORT,POLOLU_ENABLE_PIN,0x01);
-}
-
-void gpio_disable_pololu(void)
-{
-  gpio_set_pin_value(POLOLU_ENABLE_PORT,POLOLU_ENABLE_PIN,0x00);
-}
-
 void gpio_set_led(exp_led_t led_id)
 {
   switch(led_id)
   {
-    case EXP_LED0: gpio_set_pin_value(LED0_PORT,LED0_PIN,0x01);
-                   break;
-    case EXP_LED1: gpio_set_pin_value(LED1_PORT,LED1_PIN,0x01);
-                   break;
-    case EXP_LED2: gpio_set_pin_value(LED2_PORT,LED2_PIN,0x01);
-                   break;
-    case EXP_LED3: gpio_set_pin_value(LED3_PORT,LED3_PIN,0x01);
-                   break;
+    case FRONT_LEFT: HAL_GPIO_WritePin(FRONT_LEFT_GPIO_PORT, FRONT_LEFT_PIN, GPIO_PIN_SET);
+                     break;
+    case FRONT_RIGHT: HAL_GPIO_WritePin(FRONT_RIGHT_GPIO_PORT,FRONT_RIGHT_PIN,GPIO_PIN_SET);
+                      break;
+    case DOWN_LEFT_FRONT: HAL_GPIO_WritePin(DOWN_LEFT_FRONT_GPIO_PORT,DOWN_LEFT_FRONT_PIN,GPIO_PIN_SET);
+                          break;
+    case DOWN_LEFT_MIDDLE: HAL_GPIO_WritePin(DOWN_LEFT_MIDDLE_GPIO_PORT,DOWN_LEFT_MIDDLE_PIN,GPIO_PIN_SET);
+                           break;
+    case DOWN_LEFT_REAR: HAL_GPIO_WritePin(DOWN_LEFT_REAR_GPIO_PORT,DOWN_LEFT_REAR_PIN,GPIO_PIN_SET);
+                         break;
+    case DOWN_RIGHT_FRONT: HAL_GPIO_WritePin(DOWN_RIGHT_FRONT_GPIO_PORT,DOWN_RIGHT_FRONT_PIN,GPIO_PIN_SET);
+                           break;
+    case DOWN_RIGHT_MIDDLE: HAL_GPIO_WritePin(DOWN_RIGHT_MIDDLE_GPIO_PORT,DOWN_RIGHT_MIDDLE_PIN,GPIO_PIN_SET);
+                            break;
+    case DOWN_RIGHT_REAR: HAL_GPIO_WritePin(DOWN_RIGHT_REAR_GPIO_PORT,DOWN_RIGHT_REAR_PIN,GPIO_PIN_SET);
+                          break;
+    case FRONT_ANALOG: HAL_GPIO_WritePin(FRONT_ANALOG_GPIO_PORT,FRONT_ANALOG_PIN,GPIO_PIN_SET);
+                       break;
+    case DOWN_ANALOG: HAL_GPIO_WritePin(DOWN_ANALOG_GPIO_PORT,DOWN_ANALOG_PIN,GPIO_PIN_SET);
+                      break;
   }
 }
 
@@ -374,58 +145,25 @@ void gpio_clear_led(exp_led_t led_id)
 {
   switch(led_id)
   {
-    case EXP_LED0: gpio_set_pin_value(LED0_PORT,LED0_PIN,0x00);
-                   break;
-    case EXP_LED1: gpio_set_pin_value(LED1_PORT,LED1_PIN,0x00);
-                   break;
-    case EXP_LED2: gpio_set_pin_value(LED2_PORT,LED2_PIN,0x00);
-                   break;
-    case EXP_LED3: gpio_set_pin_value(LED3_PORT,LED3_PIN,0x00);
-                   break;
+    case FRONT_LEFT: HAL_GPIO_WritePin(FRONT_LEFT_GPIO_PORT,FRONT_LEFT_PIN,GPIO_PIN_RESET);
+                     break;
+    case FRONT_RIGHT: HAL_GPIO_WritePin(FRONT_RIGHT_GPIO_PORT,FRONT_RIGHT_PIN,GPIO_PIN_RESET);
+                      break;
+    case DOWN_LEFT_FRONT: HAL_GPIO_WritePin(DOWN_LEFT_FRONT_GPIO_PORT,DOWN_LEFT_FRONT_PIN,GPIO_PIN_RESET);
+                          break;
+    case DOWN_LEFT_MIDDLE: HAL_GPIO_WritePin(DOWN_LEFT_MIDDLE_GPIO_PORT,DOWN_LEFT_MIDDLE_PIN,GPIO_PIN_RESET);
+                           break;
+    case DOWN_LEFT_REAR: HAL_GPIO_WritePin(DOWN_LEFT_REAR_GPIO_PORT,DOWN_LEFT_REAR_PIN,GPIO_PIN_RESET);
+                         break;
+    case DOWN_RIGHT_FRONT: HAL_GPIO_WritePin(DOWN_RIGHT_FRONT_GPIO_PORT,DOWN_RIGHT_FRONT_PIN,GPIO_PIN_RESET);
+                           break;
+    case DOWN_RIGHT_MIDDLE: HAL_GPIO_WritePin(DOWN_RIGHT_MIDDLE_GPIO_PORT,DOWN_RIGHT_MIDDLE_PIN,GPIO_PIN_RESET);
+                            break;
+    case DOWN_RIGHT_REAR: HAL_GPIO_WritePin(DOWN_RIGHT_REAR_GPIO_PORT,DOWN_RIGHT_REAR_PIN,GPIO_PIN_RESET);
+                          break;
+    case FRONT_ANALOG: HAL_GPIO_WritePin(FRONT_ANALOG_GPIO_PORT,FRONT_ANALOG_PIN,GPIO_PIN_RESET);
+                       break;
+    case DOWN_ANALOG: HAL_GPIO_WritePin(DOWN_ANALOG_GPIO_PORT,DOWN_ANALOG_PIN,GPIO_PIN_RESET);
+                      break;
   }
 }
-
-void gpio_set_fwd_sw_pol(uint8_t pol)
-{
-  gpio_set_pin_value(LIMIT_SW_FWD_CONF_PORT,LIMIT_SW_FWD_CONF_PIN,pol);
-}
-
-void gpio_set_rvs_sw_pol(uint8_t pol)
-{
-  gpio_set_pin_value(LIMIT_SW_RVS_CONF_PORT,LIMIT_SW_RVS_CONF_PIN,pol);
-}
-
-void gpio_set_home_sw_pol(uint8_t pol)
-{
-  gpio_set_pin_value(LIMIT_SW_HOME_CONF_PORT,LIMIT_SW_HOME_CONF_PIN,pol);
-}
-
-/* input functions */
-uint8_t gpio_get_switch(exp_sw_t sw_id)
-{
-  unsigned char data;
-
-  switch(sw_id)
-  {
-    case EXP_SW0: data=gpio_sw_status&0x01;
-                  break;
-    case EXP_SW1: data=gpio_sw_status&0x02;
-                  break;
-    case EXP_SW2: data=gpio_sw_status&0x04;
-                  break;
-    case EXP_SW3: data=gpio_sw_status&0x08;
-                  break;
-    default: data=0x00;
-             break;
-  }
-  if(data) 
-    return 0x01;
-  else
-    return 0x00;
-}
-
-uint8_t gpio_get_pololu_status(void)
-{
-  return gpio_pololu_status;
-}
-
diff --git a/src/ir_feet_dyn_slave.c b/src/ir_feet_dyn_slave.c
index 8687670cd054664fef0673ef03f0700be27e89a2..1ec2beb67549cd64e6c450bfb09394aa2fcdc709 100755
--- a/src/ir_feet_dyn_slave.c
+++ b/src/ir_feet_dyn_slave.c
@@ -4,9 +4,15 @@
 #include "scheduler.h"
 #include "ram.h"
 #include "usart1.h"
+#include "gpio.h"
 
-#define DXL_DIR_Pin GPIO_PIN_5
-#define DXL_DIR_Port GPIOB
+#define DYN_RX_EN_GPIO_CLK                __GPIOC_CLK_ENABLE()
+#define DYN_RX_EN_PIN                     GPIO_PIN_14
+#define DYN_RX_EN_GPIO_PORT               GPIOC
+
+#define DYN_TX_EN_GPIO_CLK                __GPIOC_CLK_ENABLE()
+#define DYN_TX_EN_PIN                     GPIO_PIN_13
+#define DYN_TX_EN_GPIO_PORT               GPIOC
 
 /* private variables */
 TDynamixelSlave ir_feet_dyn_slave;
@@ -48,24 +54,26 @@ void ir_feet_on_ping(void)
 
 void ir_feet_set_rx(void)
 {
-  HAL_GPIO_WritePin(DXL_DIR_Port, DXL_DIR_Pin, GPIO_PIN_RESET); 
+  HAL_GPIO_WritePin(DYN_TX_EN_GPIO_PORT, DYN_TX_EN_PIN, GPIO_PIN_RESET); 
+  HAL_GPIO_WritePin(DYN_RX_EN_GPIO_PORT, DYN_RX_EN_PIN, GPIO_PIN_SET); 
 }
 
 void ir_feet_set_tx(void)
 {
-  HAL_GPIO_WritePin(DXL_DIR_Port, DXL_DIR_Pin, GPIO_PIN_SET); 
+  HAL_GPIO_WritePin(DYN_RX_EN_GPIO_PORT, DYN_RX_EN_PIN, GPIO_PIN_RESET); 
+  HAL_GPIO_WritePin(DYN_TX_EN_GPIO_PORT, DYN_TX_EN_PIN, GPIO_PIN_SET); 
 }
 
 void ir_feet_dyn_slave_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
 {
   if(ram_in_range(DEVICE_ID,address,length))
   {
-    dyn_slave_set_address(&ir_feet_dyn_slave,data[DEVICE_ID-address]);
+    dyn_slave_set_address(&ir_feet_dyn_slave,0x01/*data[DEVICE_ID-address]*/);
     ram_data[DEVICE_ID]=data[DEVICE_ID-address];
   }
   if(ram_in_range(BAUDRATE,address,length))
   {
-    ir_feet_comm_init.BaudRate=2000000/(data[BAUDRATE-address]+1);
+    ir_feet_comm_init.BaudRate=115200;//2000000/(data[BAUDRATE-address]+1);
     usart1_config(&ir_feet_dyn_slave_comm,&ir_feet_comm_init);
     ram_data[BAUDRATE]=data[BAUDRATE-address];
   }
@@ -98,19 +106,26 @@ uint8_t ir_feet_dyn_slave_init(TMemory *memory)
   TUSART_IRQ_Priorities priorities;
   GPIO_InitTypeDef GPIO_InitStruct;
 
+  DYN_RX_EN_GPIO_CLK;
+  DYN_TX_EN_GPIO_CLK;
+
   /* configure the DIR pin */
-  __HAL_RCC_GPIOB_CLK_ENABLE();
-  GPIO_InitStruct.Pin = DXL_DIR_Pin;
+  GPIO_InitStruct.Pin = DYN_TX_EN_PIN;
+  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
+  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+  HAL_GPIO_Init(DYN_TX_EN_GPIO_PORT, &GPIO_InitStruct);
+
+  GPIO_InitStruct.Pin = DYN_RX_EN_PIN;
   GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
   GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
-  HAL_GPIO_Init(DXL_DIR_Port, &GPIO_InitStruct);
+  HAL_GPIO_Init(DYN_RX_EN_GPIO_PORT, &GPIO_InitStruct);
 
   // initialize timer structure
   time_init(&ir_feet_dyn_slave_timer,ir_feet_time_get_counts_per_us(),ir_feet_time_get_counts);
 
   /* initialize the comm object */
   comm_init(&ir_feet_dyn_slave_comm,0x01,&ir_feet_dyn_slave_timer);
-  ir_feet_comm_init.BaudRate     = 2000000/(DEFAULT_BAUDRATE+1);
+  ir_feet_comm_init.BaudRate     = 115200;//2000000/(DEFAULT_BAUDRATE+1);
   ir_feet_comm_init.WordLength   = UART_WORDLENGTH_8B;
   ir_feet_comm_init.StopBits     = UART_STOPBITS_1;
   ir_feet_comm_init.Parity       = UART_PARITY_NONE;
@@ -126,7 +141,7 @@ uint8_t ir_feet_dyn_slave_init(TMemory *memory)
   priorities.dma_tx_subpriority=0;
 
   usart1_init(&ir_feet_dyn_slave_comm,&ir_feet_comm_init,&priorities);
-  dyn_slave_init(&ir_feet_dyn_slave,&ir_feet_dyn_slave_comm,DEFAULT_DEVICE_ID,DYN_VER2);
+  dyn_slave_init(&ir_feet_dyn_slave,&ir_feet_dyn_slave_comm,1/*DEFAULT_DEVICE_ID*/,DYN_VER1);
   ir_feet_dyn_slave.on_read=ir_feet_on_read;
   ir_feet_dyn_slave.on_write=ir_feet_on_write;
   ir_feet_dyn_slave.on_ping=ir_feet_on_ping;
@@ -136,6 +151,8 @@ uint8_t ir_feet_dyn_slave_init(TMemory *memory)
   dyn_slave_set_return_delay(&ir_feet_dyn_slave,DEFAULT_RETURN_DELAY);
   dyn_slave_set_return_level(&ir_feet_dyn_slave,DEFAULT_RETURN_LEVEL);
 
+  ir_feet_set_rx();
+
   /* initialize memory module */
   mem_module_init(&ir_feet_dyn_slave_mem_module);
   ir_feet_dyn_slave_mem_module.write_cmd=ir_feet_dyn_slave_write_cmd;
diff --git a/src/main.c b/src/main.c
index fe0f070483bae376febe8f85a8183d8588ef95c0..01d84674e1d0d3d4b4b1317c1e5244af59e69ed5 100755
--- a/src/main.c
+++ b/src/main.c
@@ -6,13 +6,12 @@
 #include "eeprom.h"
 #include "scheduler.h"
 #include "gpio.h"
+#include "adc_dma.h"
 
 TMemory ir_feet_memory;
 
 int main(void)
 {
-  GPIO_InitTypeDef GPIO_InitStruct;
-
   /* initialize the HAL module */
   HAL_Init();
   /* initialize EEPROM */
@@ -38,6 +37,10 @@ int main(void)
 
   while(1)/* main function does not return */
   {
+    gpio_set_led(DOWN_LEFT_FRONT);
+    HAL_Delay(1000);
+    gpio_clear_led(DOWN_LEFT_FRONT);
+    HAL_Delay(1000);
   }
 }