diff --git a/src/bioloid_stm32.c b/src/bioloid_stm32.c
index cdd23d8a4e0e1ad0b86bf195ba03b326d9cea3d3..917d7aa17b2e56c9b75e83712bbb308c4e9369de 100644
--- a/src/bioloid_stm32.c
+++ b/src/bioloid_stm32.c
@@ -79,7 +79,6 @@ uint8_t write_operation(uint8_t address,uint8_t length, uint8_t *data)
       else manager_disable_servo(j+1);
       manager_set_module(j+1,byte_value&0x07);
     }
-    else break;
   }
   if(do_op)
   {
@@ -147,7 +146,7 @@ int32_t main(void)
   EE_ReadVariable(RETURN_LEVEL_OFFSET,&eeprom_data);
   dyn_slave_set_return_level((uint8_t)eeprom_data);
   /* initialize the IMU */
-  imu_init();
+//  imu_init();
   // initialize the Analog to digital converter
 //  adc_init();
   // initialize motion manager
@@ -160,6 +159,8 @@ int32_t main(void)
   // initialize the action module
   action_init(action_period);
 
+  gpio_blink_led(NORTH_LED,1000);
+
   while(1)                             /* main function does not return */
   {
     if(dyn_slave_is_packet_ready())// check if a new instruction packet has been received
diff --git a/src/dynamixel_master_uart_dma.c b/src/dynamixel_master_uart_dma.c
index a279031de5df5c9ffb0c266bb15661c64a7631ee..e5f0e48c97e7cfc7185feac63dd6c0a5c0296031 100755
--- a/src/dynamixel_master_uart_dma.c
+++ b/src/dynamixel_master_uart_dma.c
@@ -148,6 +148,8 @@ uint8_t dyn_master_read(uint8_t id,uint8_t address,uint8_t length,uint8_t *data)
 {
   uint8_t error;
 
+  // wait for the transmission to end
+  while(dyn_master_sending_packet);
   // generate the read packet for the desired device
   dyn_init_instruction_packet(dyn_master_tx_buffer);
   // set the ping instruction
@@ -183,7 +185,9 @@ uint8_t dyn_master_read(uint8_t id,uint8_t address,uint8_t length,uint8_t *data)
 uint8_t dyn_master_write(uint8_t id, uint8_t address, uint8_t length, uint8_t *data)
 {
   uint8_t error;
-  
+   
+  // wait for the transmission to end
+  while(dyn_master_sending_packet);
   // generate the read packet for the desired device
   dyn_init_instruction_packet(dyn_master_tx_buffer);
   // set the ping instruction
@@ -537,6 +541,8 @@ void dyn_master_sync_write(uint8_t num,uint8_t *ids,uint8_t address, uint8_t len
 {
   uint8_t i;
 
+  // wait for the transmission to end
+  while(dyn_master_sending_packet);
   dyn_master_no_answer=0x01;
   // generate the sync write packet 
   dyn_init_instruction_packet(dyn_master_tx_buffer);
@@ -555,7 +561,6 @@ void dyn_master_sync_write(uint8_t num,uint8_t *ids,uint8_t address, uint8_t len
   dyn_master_enable_tx();
   // send the data
   dyn_master_send();
-  // wait for the transmission to end
 }
 
 // repeater functions
diff --git a/src/gpio.c b/src/gpio.c
index 19d621fac682a6529c9074da99beabbf485d2ed5..55969faa30bc120f7eebaadcd0f4759331b58dbf 100644
--- a/src/gpio.c
+++ b/src/gpio.c
@@ -312,6 +312,7 @@ void gpio_toggle_led(led_t led_id)
 void gpio_blink_led(led_t led_id, int16_t period_ms)
 {
   TIM_OCInitTypeDef  TIM_OCInitStructure;
+  uint16_t capture;
 
   TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_Timing;
   TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Disable;
@@ -325,6 +326,8 @@ void gpio_blink_led(led_t led_id, int16_t period_ms)
         TIM_OCInitStructure.TIM_Pulse = north_led_period;
         TIM_OC1Init(GPO_TIMER, &TIM_OCInitStructure);
         TIM_OC1PreloadConfig(GPO_TIMER, TIM_OCPreload_Disable);
+        capture = TIM_GetCounter(GPO_TIMER);
+        TIM_SetCompare1(GPO_TIMER, capture + north_led_period);
         TIM_ITConfig(GPO_TIMER, TIM_IT_CC1, ENABLE);
       }
       else
@@ -337,6 +340,8 @@ void gpio_blink_led(led_t led_id, int16_t period_ms)
         TIM_OCInitStructure.TIM_Pulse = south_led_period;
         TIM_OC2Init(GPO_TIMER, &TIM_OCInitStructure);
         TIM_OC2PreloadConfig(GPO_TIMER, TIM_OCPreload_Disable);
+        capture = TIM_GetCounter(GPO_TIMER);
+        TIM_SetCompare2(GPO_TIMER, capture + south_led_period);
         TIM_ITConfig(GPO_TIMER, TIM_IT_CC2, ENABLE);
       }
       else
@@ -349,6 +354,8 @@ void gpio_blink_led(led_t led_id, int16_t period_ms)
         TIM_OCInitStructure.TIM_Pulse = east_led_period;
         TIM_OC3Init(GPO_TIMER, &TIM_OCInitStructure);
         TIM_OC3PreloadConfig(GPO_TIMER, TIM_OCPreload_Disable);
+        capture = TIM_GetCounter(GPO_TIMER);
+        TIM_SetCompare3(GPO_TIMER, capture + east_led_period);
         TIM_ITConfig(GPO_TIMER, TIM_IT_CC3, ENABLE);
       }
       else
@@ -361,6 +368,8 @@ void gpio_blink_led(led_t led_id, int16_t period_ms)
         TIM_OCInitStructure.TIM_Pulse = west_led_period;
         TIM_OC4Init(GPO_TIMER, &TIM_OCInitStructure);
         TIM_OC4PreloadConfig(GPO_TIMER, TIM_OCPreload_Disable);
+        capture = TIM_GetCounter(GPO_TIMER);
+        TIM_SetCompare4(GPO_TIMER, capture + west_led_period);
         TIM_ITConfig(GPO_TIMER, TIM_IT_CC4, ENABLE);
       }
       else
diff --git a/src/motion_manager.c b/src/motion_manager.c
index b386bda52fca8e4973c77aa09ff80fb7bdd6fd07..cd58b087eeb37160aa48436df0e8c9e2f0f3fe03 100644
--- a/src/motion_manager.c
+++ b/src/motion_manager.c
@@ -224,7 +224,7 @@ void manager_init(uint16_t period_us)
     }
     else
     {
-      manager_servos[i].id=servo_ids[i];
+      manager_servos[i].id=0;
       manager_servos[i].model=0x0000;
       manager_servos[i].module=MM_NONE;
       manager_servos[i].encoder_resolution=0;