diff --git a/servo_firmware/rx28/src/main.c b/servo_firmware/rx28/src/main.c
index 5a55c45d23eddd5fb0596c70981e1b1c2106a57f..a8eb6f09386ce369c6e9aefa6e45f651bec08bc1 100755
--- a/servo_firmware/rx28/src/main.c
+++ b/servo_firmware/rx28/src/main.c
@@ -1,7 +1,7 @@
 #include <avr/io.h>
 #include <util/delay.h>
 #include <avr/interrupt.h>
-#include <avr/eeprom.h>
+//#include <avr/eeprom.h>
 #include "CFG_HW_Dynamixel.h"
 //#include "TXRX_Dynamixel.h"
 #include "CTRL_Dynamixel.h"
@@ -18,19 +18,26 @@ ISR( TIMER0_OVF_vect) {
 	//cli(); // disable all interrupts just to make sure
 	TorqueEnable = Read_byte_Dynamixel_RAM(Torque_Enable);
 	if (TorqueEnable == 1) {
-		ENABLE_MOTOR;
+	//if (0) {
+		//ENABLE_MOTOR;
+		DISABLE_MOTOR;
 		count++;
-		if (count == 1) {
-			//LedTOGGLE;
-			Control_Cycle();
-			HW_Security();
-			Write_Actuator();
+		count2=Read_Sensor(CTRL_Encoder_Port);
+		//if (count == 1) {
+		if (count > count2) {
+			LedTOGGLE;
+			//Control_Cycle();
+			//HW_Security();
+			//Write_Actuator();
+			count = 0;
+		}
+		if (count == 1023) {
 			count = 0;
 		}
 	} else {
 		DISABLE_MOTOR;
-		SET_CW_PWM_MOTOR(0); //PB1 => set PWM for X% duty cycle max 0x3ff o 1023 // funciona
-		SET_CCW_PWM_MOTOR(0);  //PB2 => set PWM for Y% duty cycle max 0x3ff o 1023 // funciona
+		SET_CCW_PWM_MOTOR(0); //PB1 => set PWM for X% duty cycle max 0x3ff o 1023 // funciona
+		SET_CW_PWM_MOTOR(0);  //PB2 => set PWM for Y% duty cycle max 0x3ff o 1023 // funciona
 		//LedTOGGLE;
 	}
 }
@@ -39,21 +46,22 @@ ISR( TIMER0_OVF_vect) {
 int16_t main(void) {
 	 unsigned char data[128], id, length, instruction, answer[2], status,
 			en_vector, i, j;
-	 uint16_t ii, jj;
 	// 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,
 	 43, 44, 45, 46, 67, 68, 71, 72, 73, 74, 75, 76, 77, 78, 79, 80, 83,
 	 84, 85, 86 };*/
 
-	 //   ini eeprom if first time  after run reflash system
-	if (eeprom_read_byte((uint8_t*) Gate_Restore_Eeprom) != 0xCC) {
-		 Restore_Eeprom_Factory_Values(); //once this procedure is held, no more initialization is performed
-	 }
+	//   ini eeprom if first time  after run reflash system
+	/*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();
-		// get value of Motor ID
-	//rs485_address = eepromVAR[ID];
+	//------EEPROM initial values-------------
+	/*Restore_EepromVAR();
+	 //blinkLedN(5);
+	 // get value of Motor ID
+	 rs485_address = eepromVAR[ID];*/
 
 	 	 // configure AVR chip i/o
 	Config_Hardware();
@@ -64,20 +72,8 @@ int16_t main(void) {
 
 	// end of inicialization
 	LedOFF;
-
-	ii = 0;
-	jj=1;
 	while (1) {
 
-		Write_word_Dynamixel_RAM(Goal_Position_L,ii);
-		_delay_ms(500); // funciona
-		//ii=500;
-		ii = ii + 100*jj;
-		LedTOGGLE;
-		if (ii == 900) {jj=-1;}
-		if (ii == 100) {jj=1;}
-
-
 		/*status = RxRS485Packet(&id, &instruction, &length, data);
 		 if (status == CHECK_ERROR) {
 		 TxRS485Packet(rs485_address, CHECKSUM_ERROR, 0, NULL);