diff --git a/servo_firmware/ax12/Makefile b/servo_firmware/ax12/Makefile
index 2d1411a4af7a15310e7fb0c0484a1ebdae5f4bad..8bdf19a7477384333d9c2fd04ea10ab21736832d 100755
--- a/servo_firmware/ax12/Makefile
+++ b/servo_firmware/ax12/Makefile
@@ -1,4 +1,4 @@
-PROJECT=ax12_fw
+PROJECT=rx28_fw
 ########################################################
 # afegir tots els fitxers que s'han de compilar aquí
 ########################################################
@@ -30,7 +30,7 @@ $(PROJECT).elf: $(OBJS)
 	$(CC) -c $(CFLAGS) -I$(INCLUDE_DIR) -o $@ $<
 
 download: $(MAIN_OUT_HEX)
-	../../bin/fw_downloader -d /dev/ttyUSB0 -f $(PROJECT).hex -s ax12
+	../../bin/fw_downloader -d /dev/ttyUSB0 -f $(PROJECT).hex -s rx28
 
 clean:
 	rm $(PROJECT).*
diff --git a/servo_firmware/ax12/include/CFG_HW_Dynamixel.h b/servo_firmware/ax12/include/CFG_HW_Dynamixel.h
index 1c7d6b1c6ea8ba81f528f03d5e4ac548d3b2f82f..cee708ddc68e992f8e02c5deb912d4140ef682f5 100755
--- a/servo_firmware/ax12/include/CFG_HW_Dynamixel.h
+++ b/servo_firmware/ax12/include/CFG_HW_Dynamixel.h
@@ -1,18 +1,26 @@
 #ifndef _CFG_HW_DYNAMIXEL_H
 #define _CFG_HW_DYNAMIXEL_H
 
+
+#define LED_PIN             	PD2
+#define ENABLE_MOTOR_PIN 		PD7
+#define CW_PWM_MOTOR_PIN 		PB1
+#define CCW_PWM_MOTOR_PIN 		PB2
+
 // this is for debuging tonggle LED i times
 // void blinkLedN(uint8_t i);
-// this is for debuging => tonggle LED
-//void LedTONGGLE(void) ;
-// this is for  debuging => off  LED
-//void LedOFF(void) ;
-// this is for debugging  => on   LED
-//void LedON(void) ;
-
-#define  LedOFF(x)  	PORTD |= (1<<x) 	// off the LED
-#define  LedON(x)  		PORTD &= ~(1<<x) // ON the LED
-#define  LedTOGGLE(x)  	PORTD ^= _BV(x)  // Toggle PD2 led
+
+
+#define  LedOFF  				PORTD |= (1<<LED_PIN) 	// off LED
+#define  LedON  				PORTD &= ~(1<<LED_PIN) // ON LED
+#define  LedTOGGLE   			PORTD ^= _BV(LED_PIN)  // Toggle LED
+
+#define  SET_CW_PWM_MOTOR(x)  	OCR1A = (x)   //PB1 => set PWM for X% duty cycle @ 10bit
+#define  SET_CCW_PWM_MOTOR(x)  	OCR1B = (x)  //PB2 => set PWM for Y% duty cycle @ 10bit
+
+#define  ENABLE_MOTOR			PORTD &= ~(1<<ENABLE_MOTOR_PIN) // enable motor
+#define  DISABLE_MOTOR      	PORTD |= (1<<ENABLE_MOTOR_PIN) 	// disable motor
+
 
  void Config_Hardware(void);
 
diff --git a/servo_firmware/ax12/src/CFG_HW_Dynamixel.c b/servo_firmware/ax12/src/CFG_HW_Dynamixel.c
index 1d203ef868de2132c4bdbac81172ba1aeccfbf7a..5eda0e922896e78cf5e8314c9f90ed036a45466b 100755
--- a/servo_firmware/ax12/src/CFG_HW_Dynamixel.c
+++ b/servo_firmware/ax12/src/CFG_HW_Dynamixel.c
@@ -8,13 +8,13 @@
 {
   uint8_t i;
 
-  LedOFF(PD2); // PD2 led  led OFF
+  LedOFF; //  led OFF
   _delay_ms(500);
   for (i=1; i<=(2*j); i++) {
-	 LedTOGGLE(PD2);     // tonggle PD2 led
+	 LedTOGGLE;     // tonggle led
 	 _delay_ms(200);
   }
-  LedOFF(PD2); // PD2 led  led OFF
+  LedOFF; // led OFF
   _delay_ms(500);
 }*/
 
@@ -22,12 +22,14 @@
 
 void Config_Hardware(void)
 {
-   DDRD |= (1 << PD2); // Set LED as output
-   LedON(PD2);    // ON the LED
-   DDRB |= (1 << PB1); // Set CW as output       
-    OCR1A = 0x0000;     //PB1 => set PWM for X% duty cycle @ 10bit
-   DDRB |= (1 << PB2); // Set CCW as output
-    OCR1B = 0x0000;     //PB2 => set PWM for Y% duty cycle @ 10bit
+   DDRD |= (1 << LED_PIN); // Set LED as output
+   LedON;    // ON the LED
+   DDRB |= (1 << CW_PWM_MOTOR_PIN); // Set CW pin as output
+   SET_CW_PWM_MOTOR(0x0000);  //PB1 => set PWM for 0% duty cycle @ 10bit
+   DDRB |= (1 << CCW_PWM_MOTOR_PIN); // Set CCW pin as output
+   SET_CCW_PWM_MOTOR(0x0000);  //PB2 => set PWM for 0% duty cycle @ 10bit
+   DDRD |= (1 << ENABLE_MOTOR_PIN); // Set Enable pin as output
+   DISABLE_MOTOR;	// disable motor
 
 
    cli(); // just to make sure
diff --git a/servo_firmware/ax12/src/TXRX_Dynamixel.c b/servo_firmware/ax12/src/TXRX_Dynamixel.c
index 8b061f9ee22707bc202a79b268e2d490dc25ddc6..56a5a58591633cedf1b6df51d79a614dcbd98158 100755
--- a/servo_firmware/ax12/src/TXRX_Dynamixel.c
+++ b/servo_firmware/ax12/src/TXRX_Dynamixel.c
@@ -63,8 +63,8 @@ void init_RS485(void)
 {
   // configure the IO ports
   // DDRD - Port D Data Direction Register 
-  DDRD=DDRD|0xC2;// RX_en, TX_en, TX are outputs
-  DDRD=DDRD&0xFE;// RX is an input - LED OFF 
+  DDRD=DDRD|0xC2;// RX_en, TX_en, TX are outputs 0xC2=11000010
+  DDRD=DDRD&0xFE;// RX is an input     0xFE=11111110
 
   // configure the ports to receive data
   SET_RS485_RXD;
diff --git a/servo_firmware/ax12/src/main.c b/servo_firmware/ax12/src/main.c
index af990fb2f217841e8eaf4ff315433deeff576e73..51fc747666db09c662bf93ada42fff2a4b75c0e1 100755
--- a/servo_firmware/ax12/src/main.c
+++ b/servo_firmware/ax12/src/main.c
@@ -14,7 +14,7 @@
 //*************CTRL = INTERRUPT FUNCTION *********************************
 uint16_t count = 0;
 uint16_t count2 = 0;
-ISR( TIMER0_OVF_vect) {
+/*ISR( TIMER0_OVF_vect) {
 	int16_t TorqueEnable;
 
 	sei();
@@ -38,47 +38,47 @@ ISR( TIMER0_OVF_vect) {
 		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(PD2);
+	LedOFF;
 
 	while (1) {
-		//LedOFF(PD2);
-		status = RxRS485Packet(&id, &instruction, &length, data);
+		//LedOFF;
+		/*status = RxRS485Packet(&id, &instruction, &length, data);
 		if (status == CHECK_ERROR) {
 			TxRS485Packet(rs485_address, CHECKSUM_ERROR, 0, NULL);
 		} else if (status == CORRECT) {
-			//LedON(PD2);
+			//LedON;
 			if (id == rs485_address) {
 				switch (instruction) {
 				//***********************************************************
@@ -118,7 +118,7 @@ int16_t main(void) {
 								break; //................................................
 							case 2: // case WORD
 
-								//LedTOGGLE(PD2);
+								//LedTOGGLE;
 								TxRx_Read_word_Dynamixel_RAM(data[0], answer);
 								break; //................................................
 							default:
@@ -217,9 +217,9 @@ int16_t main(void) {
 								switch (data[0]) { // aditional instructions for particular registers in ram
 								case LED: // case RAM and BYTE
 									if (Read_byte_Dynamixel_RAM(LED) == 1) {
-										LedON(PD2);
+										LedON;
 									} else {
-										LedOFF(PD2);
+										LedOFF;
 									}
 									break; //................................................
 								default:
@@ -251,7 +251,7 @@ int16_t main(void) {
 					//***************************************************
 				}
 			}
-		}
+		}*/
 	}
 }