diff --git a/servo_firmware/ax12/src/CFG_HW_Dynamixel.c b/servo_firmware/ax12/src/CFG_HW_Dynamixel.c
index 5eda0e922896e78cf5e8314c9f90ed036a45466b..10ae74a9610e43b0fa94bfab6f57d12be9fdde84 100755
--- a/servo_firmware/ax12/src/CFG_HW_Dynamixel.c
+++ b/servo_firmware/ax12/src/CFG_HW_Dynamixel.c
@@ -66,6 +66,7 @@ void Config_Hardware(void)
    /* timer/counter0 limitations.. no CTC mode.  
     *   so move starting point to where neded 
     * to do is add X */
+  TCNT0 = 0x00;
    // TCNT0 = 0x9C; // Timer/Counter Register  156 => 0.1 ms  counter value set
    
       //*****************************************
diff --git a/servo_firmware/ax12/src/TXRX_Dynamixel.c b/servo_firmware/ax12/src/TXRX_Dynamixel.c
index 56a5a58591633cedf1b6df51d79a614dcbd98158..a43eeef8da1fa4f5c1185d33cd2abdc0c25a276b 100755
--- a/servo_firmware/ax12/src/TXRX_Dynamixel.c
+++ b/servo_firmware/ax12/src/TXRX_Dynamixel.c
@@ -43,14 +43,11 @@ void RS485_send(unsigned char data)
 
 unsigned char RS485_receive(unsigned char *data)
 {
-  cli();
   if(num_data==0)
-  {
-    sei();
     return 0;
-  }
   else
   {
+    cli();
     *data=buffer_data[read_ptr];
     read_ptr++;
     num_data--;
diff --git a/servo_firmware/ax12/src/main.c b/servo_firmware/ax12/src/main.c
index 51fc747666db09c662bf93ada42fff2a4b75c0e1..ac25c6a344692fda48e18d548a852f5538e39bdf 100755
--- a/servo_firmware/ax12/src/main.c
+++ b/servo_firmware/ax12/src/main.c
@@ -14,9 +14,14 @@
 //*************CTRL = INTERRUPT FUNCTION *********************************
 uint16_t count = 0;
 uint16_t count2 = 0;
-/*ISR( TIMER0_OVF_vect) {
+
+unsigned char do_control;
+
+ISR( TIMER0_OVF_vect) {
 	int16_t TorqueEnable;
 
+        TCNT0 = 0x00;
+
 	sei();
 	TorqueEnable = Read_byte_Dynamixel_RAM(Torque_Enable);
 	if (TorqueEnable == 1) {
@@ -25,11 +30,12 @@ uint16_t count2 = 0;
 		//TIMSK &= ~( 1 << TOIE0); // disable timer0
 		count++;
 		if (count == 2) {
+                        do_control=1;
 			// 1.3s passed
 			// LedTONGGLE();
-			Control_Cycle();
-			HW_Security();
-			Write_Actuator();
+			//Control_Cycle();
+			//HW_Security();
+			//Write_Actuator();
 			count = 0;
 		}
 		//TIMSK |= ( 1 << TOIE0); // enable timer0
@@ -38,43 +44,45 @@ uint16_t count2 = 0;
 		OCR1A = CTRL_ZERO; //PB1 => set PWM for X% duty cycle
 		OCR1B = CTRL_ZERO; //PB2 => set PWM for Y% duty cycle
 	}
-}*/
+}
 
 //****************    M A I N  *********************************
 int16_t main(void) {
 	unsigned char data[128], id, length, instruction, answer[2], status,
 			en_vector, i;
 	// list of read only registers - to exclude from write
-	/*unsigned char read_only_vector[30] = { 0, 1, 2, 36, 37, 38, 39, 40, 41, 42,
+	unsigned char read_only_vector[30] = { 0, 1, 2, 36, 37, 38, 39, 40, 41, 42,
 			43, 44, 45, 46, 67, 68, 71, 72, 73, 74, 75, 76, 77, 78, 79, 80, 83,
-			84, 85, 86 };*/
+			84, 85, 86 };
 
 	//   ini eeprom if first time  after run reflash system
-	/*if (eeprom_read_byte((uint8_t*) Gate_Restore_Eeprom) != 0xCC) {
+	if (eeprom_read_byte((uint8_t*) Gate_Restore_Eeprom) != 0xCC) {
 		Restore_Eeprom_Factory_Values(); //once this procedure is held, no more initialization is performed
 		//blinkLedN(3);
-	}*/
+	}
 
 	//------EEPROM initial values-------------
-	/*Restore_EepromVAR();
+	Restore_EepromVAR();
 	//blinkLedN(5);
 	// get value of Motor ID
-	rs485_address = eepromVAR[ID];*/
+	rs485_address = eepromVAR[ID];
 
 	// configure AVR chip i/o
 	Config_Hardware();
-	/*// asign actual position to goal position and asume zero motor turns
+	// asign actual position to goal position and asume zero motor turns
 	Ini_Position();
 
 	// initialize the RS485 interface
-	init_RS485();*/
+	init_RS485();
 
 	// end of inicialization
 	LedOFF;
 
+        do_control=0;
+
 	while (1) {
 		//LedOFF;
-		/*status = RxRS485Packet(&id, &instruction, &length, data);
+		status = RxRS485Packet(&id, &instruction, &length, data);
 		if (status == CHECK_ERROR) {
 			TxRS485Packet(rs485_address, CHECKSUM_ERROR, 0, NULL);
 		} else if (status == CORRECT) {
@@ -251,7 +259,14 @@ int16_t main(void) {
 					//***************************************************
 				}
 			}
-		}*/
+		}
+                else if(do_control)
+                {
+ 			Control_Cycle();
+			HW_Security();
+			Write_Actuator();
+                        do_control=0;
+                }
 	}
 }