diff --git a/conf/airframes/motor_bench.xml b/conf/airframes/motor_bench.xml
index 59238fd4bb..2ff59a5e01 100644
--- a/conf/airframes/motor_bench.xml
+++ b/conf/airframes/motor_bench.xml
@@ -6,23 +6,24 @@ ARCHI=arm7
 
 FLASH_MODE = IAP
 
+MB=motor_bench
 
 main.ARCHDIR = $(ARCHI)
 main.ARCH = arm7tdmi
-main.TARGET = mb1
-main.TARGETDIR = mb1
+main.TARGET = mb
+main.TARGETDIR = mb
 
-main.CFLAGS += -DCONFIG=\"conf_motor_bench.h\"
-main.srcs = main_motor_bench.c
+main.CFLAGS += -DCONFIG=\"conf_motor_bench.h\" -I$(MB)
+main.srcs = $(MB)/main_motor_bench.c
 
-main.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(4e-3)'
+main.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./250.)'
 main.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
 
 main.CFLAGS += -DLED
 
 main.srcs += $(SRC_ARCH)/armVIC.c
 
-main.CFLAGS += -DUSE_UART0 -DUART0_BAUD=B38400
+main.CFLAGS += -DUSE_UART0 -DUART0_BAUD=B115200
 main.srcs += $(SRC_ARCH)/uart_hw.c
 
 main.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart0 
@@ -32,9 +33,10 @@ main.CFLAGS += -DDATALINK=PPRZ -DPPRZ_UART=Uart0
 
 main.srcs += $(SRC_ARCH)/mb_servo.c
 
+main.CFLAGS += -DUSE_TWI_CONTROLLER
 main.CFLAGS += -DI2C_SCLL=100 -DI2C_SCLH=100
 #main.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c $(SRC_ARCH)/mb_twi_controller.c
-main.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c $(SRC_ARCH)/mb_buss_twi_controller.c
+main.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c $(MB)/mb_twi_controller_asctech.c
 
 main.CFLAGS += -DMB_TACHO
 main.srcs += $(SRC_ARCH)/mb_tacho.c
diff --git a/conf/settings/motor_bench_settings.xml b/conf/settings/motor_bench_settings.xml
index 778950f946..2aaa9258d7 100644
--- a/conf/settings/motor_bench_settings.xml
+++ b/conf/settings/motor_bench_settings.xml
@@ -5,5 +5,12 @@
     <dl_settings name="trim">
 <dl_setting var="mb_modes_mode" min="0" step="1" max="4" module="mb_modes" handler="SetMode"/>
     </dl_settings>
+    <dl_settings name="twi_asctech">
+      <dl_setting var="mb_twi_controller_asctech_command_type" min="0" step="1" max="3" 
+	module="mb_twi_controller_asctech" handler="SetCommand"/>
+      <dl_setting var="mb_twi_controller_asctech_addr" min="0" step="1" max="4" 
+	module="mb_twi_controller_asctech" handler="SetAddr"/>
+    </dl_settings>
+
   </dl_settings>
 </settings>
diff --git a/sw/airborne/motor_bench/mb_twi_controller_asctech.c b/sw/airborne/motor_bench/mb_twi_controller_asctech.c
index 77ac54a9c7..a010a3508b 100644
--- a/sw/airborne/motor_bench/mb_twi_controller_asctech.c
+++ b/sw/airborne/motor_bench/mb_twi_controller_asctech.c
@@ -1,14 +1,11 @@
 #include "mb_twi_controller.h"
+#include "mb_twi_controller_asctech.h"
 
 #include "i2c.h"
 
 #include "led.h"
 
 bool_t  mb_twi_controller_asctech_command;
-#define MB_TWI_CONTROLLER_COMMAND_NONE     0
-#define MB_TWI_CONTROLLER_COMMAND_SET_ADDR 1
-#define MB_TWI_CONTROLLER_COMMAND_TEST     2
-#define MB_TWI_CONTROLLER_COMMAND_REVERSE  3
 uint8_t mb_twi_controller_asctech_command_type;
 
 #define MB_TWI_CONTROLLER_ASCTECH_ADDR_FRONT 0
@@ -16,6 +13,7 @@ uint8_t mb_twi_controller_asctech_command_type;
 #define MB_TWI_CONTROLLER_ASCTECH_ADDR_LEFT  2
 #define MB_TWI_CONTROLLER_ASCTECH_ADDR_RIGHT 3
 uint8_t mb_twi_controller_asctech_addr;
+uint8_t mb_twi_controller_asctech_new_addr;
 
 uint8_t mb_twi_nb_overun;
 uint8_t mb_twi_i2c_done;
@@ -37,6 +35,15 @@ void mb_twi_controller_set( float throttle ) {
     if (mb_twi_controller_asctech_command) {
       mb_twi_controller_asctech_command = FALSE;
       switch (mb_twi_controller_asctech_command_type) {
+
+      case MB_TWI_CONTROLLER_COMMAND_TEST :
+	i2c_buf[0] = 251;
+	i2c_buf[1] = mb_twi_controller_asctech_addr;
+	i2c_buf[2] = 0;
+	i2c_buf[3] = 231 + mb_twi_controller_asctech_addr;
+	i2c_transmit(MB_TWI_CONTROLLER_ADDR, 4, &mb_twi_i2c_done);
+      break;
+      
       case MB_TWI_CONTROLLER_COMMAND_REVERSE :
 	i2c_buf[0] = 254;
 	i2c_buf[1] = mb_twi_controller_asctech_addr;
@@ -44,6 +51,17 @@ void mb_twi_controller_set( float throttle ) {
 	i2c_buf[3] = 234 + mb_twi_controller_asctech_addr;
 	i2c_transmit(MB_TWI_CONTROLLER_ADDR, 4, &mb_twi_i2c_done);
 	break;
+   
+    case MB_TWI_CONTROLLER_COMMAND_SET_ADDR :
+	i2c_buf[0] = 250;
+	i2c_buf[1] = mb_twi_controller_asctech_addr;
+	i2c_buf[2] = mb_twi_controller_asctech_new_addr;
+	i2c_buf[3] = 230 + mb_twi_controller_asctech_addr + 
+	             mb_twi_controller_asctech_new_addr;
+	mb_twi_controller_asctech_addr = mb_twi_controller_asctech_new_addr;
+	i2c_transmit(MB_TWI_CONTROLLER_ADDR, 4, &mb_twi_i2c_done);
+	break;
+
       }
     }
     else {
diff --git a/sw/airborne/motor_bench/mb_twi_controller_asctech.h b/sw/airborne/motor_bench/mb_twi_controller_asctech.h
index f6bc66d59a..c48b523738 100644
--- a/sw/airborne/motor_bench/mb_twi_controller_asctech.h
+++ b/sw/airborne/motor_bench/mb_twi_controller_asctech.h
@@ -1,12 +1,28 @@
 #ifndef MB_TWI_CONTROLLER_ASCTECH_H
 #define MB_TWI_CONTROLLER_ASCTECH_H
 
+
+#define MB_TWI_CONTROLLER_COMMAND_NONE     0
+#define MB_TWI_CONTROLLER_COMMAND_TEST     1
+#define MB_TWI_CONTROLLER_COMMAND_REVERSE  2
+#define MB_TWI_CONTROLLER_COMMAND_SET_ADDR 3
+
+
 extern bool_t  mb_twi_controller_asctech_command;
 extern uint8_t mb_twi_controller_asctech_command_type;
+extern uint8_t mb_twi_controller_asctech_addr;
+extern uint8_t mb_twi_controller_asctech_new_addr;
 
 #define mb_twi_controller_asctech_SetCommand(value) { \
     mb_twi_controller_asctech_command = TRUE;	      \
     mb_twi_controller_asctech_command_type = value;   \
   }
 
+#define mb_twi_controller_asctech_SetAddr(value) {			\
+    mb_twi_controller_asctech_command = TRUE;				\
+    mb_twi_controller_asctech_command_type = MB_TWI_CONTROLLER_COMMAND_SET_ADDR; \
+    mb_twi_controller_asctech_new_addr = value;				\
+  }
+
+
 #endif /* MB_TWI_CONTROLLER_ASCTECH_H */