diff --git a/communications/Makefile b/communications/Makefile
index cd5fc84e7adda7128bbee79ec44176ac7c8ee20f..17a0a643714bc0e6bb803b19868e0fcc25a251ae 100644
--- a/communications/Makefile
+++ b/communications/Makefile
@@ -7,6 +7,7 @@ SOURCES=src/dynamixel.c src/dynamixel_master.c src/serial_console.c
 OBJS=$(SOURCES:.c=.o)
 SRC_DIR=./src/
 INCLUDE_DIR=./include/
+CONT_DIR=../controllers
 BUILD_DIR=./build/
 BIN_DIR=./bin/
 LIB_DIR=./lib/
@@ -38,7 +39,7 @@ $(PROJECT).a: $(OBJS)
 	mkdir -p lib
 	$(OBJCOPY) $(ARFLAGS) $(LIB_DIR)$(PROJECT).a $(OBJS)
 %.o:%.c
-	$(CC) -c $(CFLAGS) -I$(INCLUDE_DIR) -o $@ $<
+	$(CC) -c $(CFLAGS) -I$(INCLUDE_DIR) -I$(CONT_DIR)/include -o $@ $<
 examples:
 	$(MAKE) -C src/examples	
 clean:
diff --git a/communications/src/dynamixel_master.c b/communications/src/dynamixel_master.c
index c1b445ac1b8fc594fa0d31aba5c2a31e909c09a3..a845cba475032de6e19183814a80f2b249834fa6 100644
--- a/communications/src/dynamixel_master.c
+++ b/communications/src/dynamixel_master.c
@@ -1,3 +1,4 @@
+#include "cm510_cfg.h"
 #include "comm_cfg.h"
 #include "dynamixel_master.h"
 
@@ -116,11 +117,6 @@ uint8_t dyn_master_wait_transmission(void)
 
 uint8_t dyn_master_send(void)
 {
-  uint8_t error;
-
-  // wait until any previous transmission ends
-  if((error=dyn_master_wait_transmission())!=DYN_SUCCESS)
-    return error;
   // set the transfer
   dyn_master_sent_bytes=1;
   UDR0=dyn_master_tx_buffer[0];
@@ -268,6 +264,8 @@ uint8_t dyn_master_ping(uint8_t id)
 {
   uint8_t error;
 
+  // wait until any previous transmissions ends
+  dyn_master_wait_transmission();
   // generate the ping packet for the desired device
   dyn_init_ping_packet(dyn_master_tx_buffer,id);
   dyn_master_rx_num_packets=0x01;
@@ -313,6 +311,8 @@ uint8_t dyn_master_read_table(uint8_t id,uint16_t address,uint16_t length,uint8_
 {
   uint8_t error;
 
+  // wait until any previous transmissions ends
+  dyn_master_wait_transmission();
   // generate the read packet for the desired device
   dyn_init_read_packet(dyn_master_tx_buffer,id,address,length);
   dyn_master_rx_num_packets=0x01;
@@ -365,6 +365,8 @@ uint8_t dyn_master_write_table(uint8_t id, uint16_t address, uint16_t length, ui
 {
   uint8_t error;
 
+  // wait until any previous transmissions ends
+  dyn_master_wait_transmission();
   // generate the write packet for the desired device
   dyn_init_write_packet(dyn_master_tx_buffer,id,address,length,data);
   dyn_master_rx_num_packets=0x01;
@@ -397,6 +399,8 @@ uint8_t dyn_master_reg_write(uint8_t id, uint16_t address, uint16_t length, uint
 {
   uint8_t error;
 
+  // wait until any previous transmissions ends
+  dyn_master_wait_transmission();
   // generate the registered write packet for the desired device
   dyn_init_reg_write_packet(dyn_master_tx_buffer,id,address,length,data);
   dyn_master_rx_num_packets=0x01;
@@ -429,6 +433,8 @@ uint8_t dyn_master_action(void)
 {
   uint8_t error;
 
+  // wait until any previous transmissions ends
+  dyn_master_wait_transmission();
   // generate the action packet for the desired device
   dyn_init_action_packet(dyn_master_tx_buffer);
   dyn_master_rx_num_packets=0x01;
@@ -455,6 +461,8 @@ uint8_t dyn_master_sync_write(uint8_t num,uint8_t *ids,uint16_t address, uint16_
 {
   uint8_t error;
 
+  // wait until any previous transmissions ends
+  dyn_master_wait_transmission();
   // generate the write packet for the desired device
   dyn_init_sync_write_packet(dyn_master_tx_buffer,num,ids,address,length,data);
   dyn_master_rx_num_packets=0x01;
@@ -481,6 +489,8 @@ uint8_t dyn_master_bulk_read(uint8_t num,uint8_t *ids,uint16_t *address, uint16_
 {
   uint8_t error,i,ver1_address[255],ver1_length[255];
 
+  // wait until any previous transmissions ends
+  dyn_master_wait_transmission();
   // generate the read packet for the desired device
   for(i=0;i<num;i++)
   {
diff --git a/controllers/include/cm510.h b/controllers/include/cm510.h
index c4071376f552080b91dcc553227c7ed220051fd5..55c10effa82c693e9661f99555b873ad2d477266 100755
--- a/controllers/include/cm510.h
+++ b/controllers/include/cm510.h
@@ -32,6 +32,7 @@ extern "C" {
 #include "action.h"
 #include "user_time.h"
 #include "serial_console.h"
+#include "exp_board.h"
 
 // general interface
 /**
diff --git a/controllers/include/cm510_cfg.h b/controllers/include/cm510_cfg.h
index 776fe305ba4217b94218a925ff3cde3295e0dbec..88cca06cd00138d8c4dc919d5551b578379bc297 100644
--- a/controllers/include/cm510_cfg.h
+++ b/controllers/include/cm510_cfg.h
@@ -38,4 +38,8 @@
 #define BALANCE_ANKLE_ROLL_GAIN                 (4.0/20.0)
 #define BALANCE_HIP_ROLL_GAIN                   (4.0/40.0)
 
+// expansion board configuration parameters
+#define EXP_BOARD_USE_LOOP                      1
+#define EXP_BOARD_ID                            192
+
 #endif
diff --git a/controllers/src/adc.c b/controllers/src/adc.c
index d977771bd003a75f0750ab80dbc9efdb941fff06..28ad8059b0a28633ca901bb16debc5b037769331 100644
--- a/controllers/src/adc.c
+++ b/controllers/src/adc.c
@@ -21,7 +21,10 @@ void adc_set_channel(uint8_t ch_id)
 {
   ADMUX&=0xF8;
   ADMUX|=(ch_id&0x07);
-  PORTA=~(0x80>>ch_id);// enable the desired channel
+  if(ch_id==0)
+    PORTA&=0xFC;
+  else
+    PORTA=~(0x80>>(ch_id-1));// enable the desired channel
 }
 
 void adc_set_sample_period(void)
diff --git a/controllers/src/cm510.c b/controllers/src/cm510.c
index 45c9915acca61b717e3309f1e844e8a9f9722c47..dd68036f9fe963d9ab08e8668037c1e8929373b8 100755
--- a/controllers/src/cm510.c
+++ b/controllers/src/cm510.c
@@ -21,6 +21,7 @@
 #include <util/delay.h>
 #include "cm510_cfg.h"
 #include "cm510.h"
+#include "exp_board.h"
 
 /* external user functions */
 extern void user_init(void);
@@ -41,6 +42,7 @@ void init_cm510(void)
   init_adc();
   init_buzzer();
   init_user_time();
+  exp_board_init(EXP_BOARD_ID);
 }
 
 int16_t main(void)
@@ -48,8 +50,8 @@ int16_t main(void)
   init_cm510();
   sei();
   /* call the user initialization function */
-  user_init();
   manager_init(18);
+  user_init();
   // turn BAT_LED on to indicate the initialization is done
   turn_led_on(LED_BAT);
 
diff --git a/dyn_devices/Makefile b/dyn_devices/Makefile
index a1e46e346161ce7dbf6a19e13bfc33a2501dfaca..c0bcc5b33104e21947c6a0c2281013734ca8498d 100755
--- a/dyn_devices/Makefile
+++ b/dyn_devices/Makefile
@@ -5,11 +5,12 @@ SRC_DIR=./src/
 BIN_DIR=./build/
 LIB_DIR=./lib/
 COMM_DIR=../communications
+CONT_DIR=../controllers
 CC=avr-gcc
 OBJCOPY=avr-ar
 MMCU=atmega2561
 
-INC_DIRS=-I./include/ -I$(COMM_DIR)/include
+INC_DIRS=-I./include/ -I$(COMM_DIR)/include -I$(CONT_DIR)/include
 
 LIBS=$(COMM_DIR)/lib/libcomm.a
 
diff --git a/dyn_devices/include/exp_board.h b/dyn_devices/include/exp_board.h
index 4e2da9b9166b68aeb70cc3eeae5ec710ad23d767..9fae29e5a1773f53d55c9bfb4b63a0c29304b0e6 100755
--- a/dyn_devices/include/exp_board.h
+++ b/dyn_devices/include/exp_board.h
@@ -40,6 +40,7 @@ typedef enum {DAC0=0,DAC1} dac_id_t;
 typedef enum {ADC0=0,ADC1,ADC2,ADC3,ADC4,ADC5,ADC6,ADC7} adc_id_t;
 
 uint8_t exp_board_init(uint8_t board_id);
+uint8_t exp_board_is_present(void);
 
 /* ADC interface */
 uint8_t exp_adc_start(void);
diff --git a/dyn_devices/include/exp_board_reg.h b/dyn_devices/include/exp_board_reg.h
index 997ac62bbec7186d9746b64c3391c8522351089f..d8e30c7b234502c2077dabde6232edc1061f353f 100755
--- a/dyn_devices/include/exp_board_reg.h
+++ b/dyn_devices/include/exp_board_reg.h
@@ -180,6 +180,66 @@
 #define UART_USB_BAUDRATE_L       0x94
 #define UART_USB_BAUDRATE_H       0x95
 
-#define RAM_SIZE                  150
+// block read of all the expansion board data
+#define BLOCK_START_ADDRESS             0x96
+#define BLOCK_LENGTH                    55
+
+#define GPIO0_data_block                0x96   
+#define GPIO1_data_block                0x97   
+#define GPIO2_data_block                0x98   
+#define GPIO3_data_block                0x99   
+#define GPIO4_data_block                0x9A   
+#define GPIO5_data_block                0x9B   
+#define GPIO6_data_block                0x9C   
+#define GPIO7_data_block                0x9D   
+#define GPIO8_data_block                0x9E   
+#define GPIO9_data_block                0x9F   
+#define GPIO10_data_block               0xA0   
+#define GPIO11_data_block               0xA1   
+#define GPIO12_data_block               0xA2   
+#define GPIO13_data_block               0xA3   
+#define GPIO14_data_block               0xA4   
+#define GPIO15_data_block               0xA5   
+#define GPIO16_data_block               0xA6   
+#define GPIO17_data_block               0xA7   
+#define GPIO18_data_block               0xA8   
+#define ADC_CHANNEL0_L_block            0xA9
+#define ADC_CHANNEL0_H_block            0xAA
+#define ADC_CHANNEL1_L_block            0xAB
+#define ADC_CHANNEL1_H_block            0xAC
+#define ADC_CHANNEL2_L_block            0xAD
+#define ADC_CHANNEL2_H_block            0xAE
+#define ADC_CHANNEL3_L_block            0xAF
+#define ADC_CHANNEL3_H_block            0xB0
+#define ADC_CHANNEL4_L_block            0xB1
+#define ADC_CHANNEL4_H_block            0xB2
+#define ADC_CHANNEL5_L_block            0xB3
+#define ADC_CHANNEL5_H_block            0xB4
+#define ADC_CHANNEL6_L_block            0xB5
+#define ADC_CHANNEL6_H_block            0xB6
+#define ADC_CHANNEL7_L_block            0xB7
+#define ADC_CHANNEL7_H_block            0xB8
+#define ADC_AVG_CHANNEL0_L_block        0xB9
+#define ADC_AVG_CHANNEL0_H_block        0xBA
+#define ADC_AVG_CHANNEL1_L_block        0xBB
+#define ADC_AVG_CHANNEL1_H_block        0xBC
+#define ADC_AVG_CHANNEL2_L_block        0xBD
+#define ADC_AVG_CHANNEL2_H_block        0xBE
+#define ADC_AVG_CHANNEL3_L_block        0xBF
+#define ADC_AVG_CHANNEL3_H_block        0xC0
+#define ADC_AVG_CHANNEL4_L_block        0xC1
+#define ADC_AVG_CHANNEL4_H_block        0xC2
+#define ADC_AVG_CHANNEL5_L_block        0xC3
+#define ADC_AVG_CHANNEL5_H_block        0xC4
+#define ADC_AVG_CHANNEL6_L_block        0xC5
+#define ADC_AVG_CHANNEL6_H_block        0xC6
+#define ADC_AVG_CHANNEL7_L_block        0xC7
+#define ADC_AVG_CHANNEL7_H_block        0xC8
+#define COMPASS_HEADING_VAL_L_block     0xC9
+#define COMPASS_HEADING_VAL_H_block     0xCA
+#define COMPASS_AVG_HEADING_VAL_L_block 0xCB
+#define COMPASS_AVG_HEADING_VAL_H_block 0xCC
+
+#define RAM_SIZE                  205
 
 #endif
diff --git a/dyn_devices/src/exp_board/exp_board.c b/dyn_devices/src/exp_board/exp_board.c
index bcb85b73c4102e7d77c00cad4cdc1627010155db..7f18d59d7177c63dbb0accf8c5fc66ed31138f5f 100755
--- a/dyn_devices/src/exp_board/exp_board.c
+++ b/dyn_devices/src/exp_board/exp_board.c
@@ -15,11 +15,20 @@
  *  along with iri-libbioloid.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#include "cm510_cfg.h"
 #include "exp_board_reg.h"
 #include "exp_board.h"
 #include "dynamixel_master.h"
 #include <util/delay.h>
 
+#ifndef EXP_BOARD_USE_LOOP
+  #define EXP_BOARD_USE_LOOP     0
+#endif
+
+#ifndef EXP_BOARD_ID
+  #define EXP_BOARD_ID           192
+#endif
+
 uint8_t exp_board_id;
 uint8_t exp_board_present;
 uint8_t exp_board_dac_present;
@@ -27,11 +36,15 @@ uint8_t exp_board_uart_ttl_present;
 uint8_t exp_board_pwm_present;
 uint8_t exp_board_uart_usb_present;
 
+uint8_t exp_board_int_data[BLOCK_LENGTH];
+
 /* private functions */
+#if EXP_BOARD_USE_LOOP==1
 void exp_board_loop(void)
 {
-  
+  dyn_master_read_table(exp_board_id,BLOCK_START_ADDRESS,BLOCK_LENGTH,exp_board_int_data);
 }
+#endif
 
 // public functions
 // expansion board initialize
@@ -90,6 +103,11 @@ uint8_t exp_board_init(uint8_t board_id)
   }
 }
 
+uint8_t exp_board_is_present(void)
+{  
+  return exp_board_present;
+}
+
 /* ADC interface */
 uint8_t exp_adc_start(void)
 {
@@ -160,22 +178,30 @@ uint8_t exp_adc_get_sample_period(void)
 
 uint16_t exp_adc_get_channel(adc_id_t channel_id)
 {
+  #if EXP_BOARD_USE_LOOP==1
+    return exp_board_int_data[ADC_CHANNEL0_L_block+channel_id*2-BLOCK_START_ADDRESS]+exp_board_int_data[ADC_CHANNEL0_H_block+channel_id*2-BLOCK_START_ADDRESS]*256;
+  #else
   uint16_t voltage;
 
   if(dyn_master_read_word(exp_board_id,ADC_CHANNEL0_L+channel_id*2,&voltage)==DYN_SUCCESS)
     return ((uint32_t)voltage*5000)/1024;
   else
     return 0xFF;
+  #endif
 }
 
 uint16_t exp_adc_get_avg_channel(adc_id_t channel_id)
 {
+  #if EXP_BOARD_USE_LOOP==1
+    return exp_board_int_data[ADC_AVG_CHANNEL0_L_block+channel_id*2-BLOCK_START_ADDRESS]+exp_board_int_data[ADC_AVG_CHANNEL0_H_block+channel_id*2-BLOCK_START_ADDRESS]*256;
+  #else
   uint16_t voltage;
-
+ 
   if(dyn_master_read_word(exp_board_id,ADC_AVG_CHANNEL0_L+channel_id*2,&voltage)==DYN_SUCCESS)
     return ((uint32_t)voltage*5000)/1024;
   else
     return 0xFF;
+  #endif
 }
 
 /* GPIO interface */
@@ -191,6 +217,9 @@ uint8_t exp_gpio_config(gpio_id_t gpio_id, uint8_t dir)
 
 uint8_t exp_gpio_get_value(gpio_id_t gpio_id)
 {
+  #if EXP_BOARD_USE_LOOP==1
+    return exp_board_int_data[GPIO0_data_block+gpio_id-BLOCK_START_ADDRESS];
+  #else
   uint8_t level;
 
   if(gpio_id<GPIO0 || gpio_id>GPIO18)
@@ -199,6 +228,7 @@ uint8_t exp_gpio_get_value(gpio_id_t gpio_id)
     return level;
   else
     return 0xFF;
+  #endif
 }
 
 uint8_t exp_gpio_set_value(gpio_id_t gpio_id, uint8_t level)
@@ -595,22 +625,30 @@ uint8_t exp_compass_is_calibrating(void)
 
 uint16_t exp_compass_get_heading(void)
 {
+  #if EXP_BOARD_USE_LOOP==1
+    return exp_board_int_data[COMPASS_HEADING_VAL_L_block-BLOCK_START_ADDRESS]+exp_board_int_data[COMPASS_HEADING_VAL_H_block-BLOCK_START_ADDRESS]*256;
+  #else
   uint16_t heading;
 
   if(dyn_master_read_word(exp_board_id,COMPASS_HEADING_VAL_L,&heading)==DYN_SUCCESS)
     return heading;
   else
     return 0xFFFF;
+  #endif
 }
 
 uint16_t exp_compass_get_avg_heading(void)
 {
+  #if EXP_BOARD_USE_LOOP==1
+    return exp_board_int_data[COMPASS_AVG_HEADING_VAL_L_block-BLOCK_START_ADDRESS]+exp_board_int_data[COMPASS_AVG_HEADING_VAL_H_block-BLOCK_START_ADDRESS]*256;
+  #else
   uint16_t heading;
 
   if(dyn_master_read_word(exp_board_id,COMPASS_AVG_HEADING_VAL_L,&heading)==DYN_SUCCESS)
     return heading;
   else
     return 0xFFFF;
+  #endif
 }
 
 uint8_t exp_compass_set_avg_samples(uint8_t num_samples)
@@ -963,4 +1001,3 @@ uint8_t exp_uart_usb_receive_data(void)
   else
     return 0xFF;
 }
-
diff --git a/motion/src/examples/walk_straight.c b/motion/src/examples/walk_straight.c
index 358f50b66809ef47557159a2193121c039c050c3..00a353b6b561d7701831dc656c56f741da93385b 100644
--- a/motion/src/examples/walk_straight.c
+++ b/motion/src/examples/walk_straight.c
@@ -111,10 +111,10 @@ void user_loop(void)
                             state=wait_walk_ready;
                           else
                           {
-                            initial_heading=exp_compass_get_avg_heading();
-                            action_set_page(32);
-                            action_start_page();
-                            state=check_compass;
+//                            initial_heading=exp_compass_get_avg_heading();
+//                            action_set_page(32);
+//                            action_start_page();
+//                            state=check_compass;
                           }
                           break;
     case check_compass: if((increment_heading=get_avg_heading_inc(initial_heading))>200 || (increment_heading=get_avg_heading_inc(initial_heading))<-200)
diff --git a/motion/src/motion_manager.c b/motion/src/motion_manager.c
index 94653479b8da21eedc6b5971f05ba085f45a207b..ea71d59629ea9cefa857d5db77a607826f5be3dc 100644
--- a/motion/src/motion_manager.c
+++ b/motion/src/motion_manager.c
@@ -1,6 +1,7 @@
 #include <avr/io.h>
 #include <avr/interrupt.h>
 #include <util/delay.h>
+#include "cm510_cfg.h"
 #include "motion_cfg.h"
 #include "motion_manager.h"
 #include "motion_pages.h"
@@ -10,6 +11,7 @@
 #include "action.h"
 #include "balance.h"
 #include "buzzer.h"
+#include "exp_board.h"
 
 // external functions
 extern void buzzer_start_alarm(note_t note,uint16_t on_time_100ms,uint16_t off_time_100ms);
@@ -18,6 +20,7 @@ extern void balance_loop(void);
 extern void balance_get_all_offsets(int16_t **offsets);
 extern void action_init(void);
 extern void action_process(void);
+extern void exp_board_loop(void);
 
 // private variables
 uint8_t manager_num_servos;
@@ -165,6 +168,10 @@ void manager_loop(void)
 {
   if(manager_period_done()==0x01)
   {
+    #if EXP_BOARD_USE_LOOP==1
+      if(exp_board_is_present())
+        exp_board_loop(); 
+    #endif
     // call the action process
     action_process(); //action_ready
     // balance the robot