diff --git a/Firmware/Behavior/app.c b/Firmware/Behavior/app.c
index a858827..5472e82 100644
--- a/Firmware/Behavior/app.c
+++ b/Firmware/Behavior/app.c
@@ -36,7 +36,7 @@ void hwbp_app_initialize(void)
     uint8_t hwH = 2;
     uint8_t hwL = 0;
     uint8_t fwH = 3;
-    uint8_t fwL = 2;
+    uint8_t fwL = 3;
     uint8_t ass = 0;    
     
    	/* Start core */
@@ -62,7 +62,7 @@ void core_callback_catastrophic_error_detected(void)
 {
     uint8_t led[3] = {0, 0, 0};
         
-	 timer_type0_stop(&TCF0); clr_DO0;
+	timer_type0_stop(&TCF0); clr_DO0;
     timer_type0_stop(&TCE0); clr_DO1;
     timer_type0_stop(&TCD0); clr_DO2;
     timer_type0_stop(&TCC0); clr_DO3;
@@ -180,7 +180,9 @@ void core_callback_reset_registers(void)
     //app_regs.REG_PORT_DIOS_IN = 0;
     
     //app_regs.REG_DATA[0] = 0;
-	 app_regs.REG_DATA[1] = 0;
+	 app_regs.REG_DATA_ENCODERS[REG_DATA_ENCODERS_INDEX_ENCODER0] = 0;
+	 app_regs.REG_DATA_ENCODERS[REG_DATA_ENCODERS_INDEX_ENCODER1] = 0;
+	 app_regs.REG_DATA_ENCODERS[REG_DATA_ENCODERS_INDEX_ENCODER2] = 0;
     
     app_regs.REG_OUTPUT_PULSE_EN = B_PORT0_12V | B_PORT1_12V | B_PORT2_12V;
 
@@ -363,45 +365,98 @@ uint8_t t1ms = 0;
 
 bool first_adc_channel;
 
+int16_t previous_encoder_poke0;
+int16_t previous_encoder_poke1;
 int16_t previous_encoder_poke2;
 
+int16_t encoder0_value;
+int16_t encoder1_value;
+int16_t encoder2_value;
+
 void core_callback_t_before_exec(void)
 {
-   if (t1ms++ & 1)
-   {
-       /* Read ADC */
-       core_func_mark_user_timestamp();
+	if (t1ms++ & 1)
+	{
+		/* Read ADC */
+		core_func_mark_user_timestamp();
        
-       /* Start conversation on ADCA Channel 0*/
-       first_adc_channel = true;
-		 ADCA_CH0_MUXCTRL = 0 << 3;
-       ADCA_CH0_CTRL |= ADC_CH_START_bm;
+		/* Start conversation on ADCA Channel 0*/
+		first_adc_channel = true;
+		ADCA_CH0_MUXCTRL = 0 << 3;
+		ADCA_CH0_CTRL |= ADC_CH_START_bm;
        
-       /* Read encoder on Port 2 */
-       if (app_regs.REG_EN_ENCODERS & B_EN_ENCODER_PORT2)
-       {
-           int16_t timer_cnt = TCD1_CNT;
+	   	/* Read encoder on Port 0 */
+	   	if (app_regs.REG_EN_ENCODERS & B_EN_ENCODER_PORT0)
+	   	{
+		   	int16_t timer_cnt = TCD1_CNT;
+		   		
+		   	if (app_regs.REG_CONF_ENCODERS == GM_POSITION)
+		   	{
+			   	if (timer_cnt > 32768)
+			   	{
+				   	app_regs.REG_DATA_ENCODERS[REG_DATA_ENCODERS_INDEX_ENCODER0] = 0xFFFF - timer_cnt;
+				   	encoder0_value = 0xFFFF - timer_cnt;
+			   	}
+			   	else
+			   	{
+				   	app_regs.REG_DATA_ENCODERS[REG_DATA_ENCODERS_INDEX_ENCODER0] = (32768 - timer_cnt) * -1;
+			   	}
+		   	}
+		   	else
+		   	{
+			   	app_regs.REG_DATA_ENCODERS[REG_DATA_ENCODERS_INDEX_ENCODER0] = previous_encoder_poke0 - timer_cnt;
+			   	previous_encoder_poke0 = timer_cnt;
+		   	}
+	   	}
+
+		/* Read encoder on Port 1 */
+		if (app_regs.REG_EN_ENCODERS & B_EN_ENCODER_PORT1)
+		{
+			int16_t timer_cnt = TCE1_CNT;
+			
+			if (app_regs.REG_CONF_ENCODERS == GM_POSITION)
+			{
+				if (timer_cnt > 32768)
+				{
+					app_regs.REG_DATA_ENCODERS[REG_DATA_ENCODERS_INDEX_ENCODER1] = 0xFFFF - timer_cnt;
+					encoder1_value = 0xFFFF - timer_cnt;
+				}
+				else
+				{
+					app_regs.REG_DATA_ENCODERS[REG_DATA_ENCODERS_INDEX_ENCODER1] = (32768 - timer_cnt) * -1;
+				}
+			}
+			else
+			{
+				app_regs.REG_DATA_ENCODERS[REG_DATA_ENCODERS_INDEX_ENCODER1] = previous_encoder_poke1 - timer_cnt;
+				previous_encoder_poke1 = timer_cnt;
+			}
+		}
+	   
+		/* Read encoder on Port 2 */
+		if (app_regs.REG_EN_ENCODERS & B_EN_ENCODER_PORT2)
+		{
+			int16_t timer_cnt = TCF1_CNT;
            
-           if (app_regs.REG_CONF_ENCODERS == GM_POSITION)
-			  {               
-               if (timer_cnt > 32768)
-               {
-                   app_regs.REG_DATA[1] = 0xFFFF - timer_cnt;
-               }
-               else
-               {
-					    app_regs.REG_DATA[1] = (32768 - timer_cnt) * -1;
-               }
-			  }
-			  else
-			  {
-			        app_regs.REG_DATA[1] = previous_encoder_poke2 - timer_cnt;			        
-					  
-			        previous_encoder_poke2 = timer_cnt;
-			  }
-			  
-       } 
-   }      
+			if (app_regs.REG_CONF_ENCODERS == GM_POSITION)
+			{               
+			if (timer_cnt > 32768)
+			{
+				app_regs.REG_DATA_ENCODERS[REG_DATA_ENCODERS_INDEX_ENCODER2] = 0xFFFF - timer_cnt;
+				encoder2_value = 0xFFFF - timer_cnt;
+			}
+			else
+			{
+				app_regs.REG_DATA_ENCODERS[REG_DATA_ENCODERS_INDEX_ENCODER2] = (32768 - timer_cnt) * -1;
+			}
+			}
+			else
+			{
+				app_regs.REG_DATA_ENCODERS[REG_DATA_ENCODERS_INDEX_ENCODER2] = previous_encoder_poke2 - timer_cnt;			        					  
+				previous_encoder_poke2 = timer_cnt;
+			}			  
+		}
+	}      
 }
 
 void core_callback_t_after_exec(void){}	
@@ -517,10 +572,16 @@ void core_callback_t_1ms(void)
 	if (int2_enable_counter)
 		if ((--int2_enable_counter) == 0)
 			PORTF_INTCTRL |= INT_LEVEL_LOW;
+			
+	// If any of the encoders is enabled, send the data
+	if (app_regs.REG_EN_ENCODERS)
+	{
+		core_func_send_event(ADD_REG_DATA_ENCODERS, false);
+	}			
 }
 
 /************************************************************************/
-/* Callbacks: cloc control                                              */
+/* Callbacks: clock control                                              */
 /************************************************************************/
 void core_callback_clock_to_repeater(void) {}
 void core_callback_clock_to_generator(void) {}
diff --git a/Firmware/Behavior/app_funcs.c b/Firmware/Behavior/app_funcs.c
index bbe990c..d1c5989 100644
--- a/Firmware/Behavior/app_funcs.c
+++ b/Firmware/Behavior/app_funcs.c
@@ -73,7 +73,7 @@ void (*app_func_rd_pointer[])(void) = {
 	&app_read_REG_DIS_SERVOS,
 	&app_read_REG_EN_ENCODERS,
 	&app_read_REG_CONF_ENCODERS,
-	&app_read_REG_RESERVED2,
+	&app_read_REG_DATA_ENCODERS,
 	&app_read_REG_RESERVED3,
 	&app_read_REG_RESERVED4,
 	&app_read_REG_RESERVED5,
@@ -167,7 +167,7 @@ bool (*app_func_wr_pointer[])(void*) = {
 	&app_write_REG_DIS_SERVOS,
 	&app_write_REG_EN_ENCODERS,
 	&app_write_REG_CONF_ENCODERS,
-	&app_write_REG_RESERVED2,
+	&app_write_REG_DATA_ENCODERS,
 	&app_write_REG_RESERVED3,
 	&app_write_REG_RESERVED4,
 	&app_write_REG_RESERVED5,
@@ -600,15 +600,6 @@ bool app_write_REG_PORT_DIOS_IN(void *a)
 void app_read_REG_DATA(void) {}      // The register is always updated
 bool app_write_REG_DATA(void *a)     
 {
-	uint16_t *reg = ((uint16_t*)a);
-
-	app_regs.REG_DATA[1] = reg[1];   // Write only to encoder counter
-
-	if (_states_.quad_counter.port2)
-	{
-		TCD1_CNT = 0x8000 + reg[1];	// Write only to encoder counter
-	}
-    
 	return true;
 }
 
@@ -1469,12 +1460,22 @@ bool app_write_REG_DIS_SERVOS(void *a)
 /************************************************************************/
 /* REG_EN_ENCODERS                                                      */
 /************************************************************************/
+extern int16_t previous_encoder_poke0;
+extern int16_t previous_encoder_poke1;
 extern int16_t previous_encoder_poke2;
 
 void app_read_REG_EN_ENCODERS(void)
 {
 	app_regs.REG_EN_ENCODERS = 0;
     
+    if(_states_.quad_counter.port0)
+    {
+	    app_regs.REG_EN_ENCODERS |= B_EN_ENCODER_PORT0;
+    }
+    if(_states_.quad_counter.port1)
+    {
+	    app_regs.REG_EN_ENCODERS |= B_EN_ENCODER_PORT1;
+    }
     if(_states_.quad_counter.port2)
     {
         app_regs.REG_EN_ENCODERS |= B_EN_ENCODER_PORT2;
@@ -1484,54 +1485,148 @@ void app_read_REG_EN_ENCODERS(void)
 bool app_write_REG_EN_ENCODERS(void *a)
 {
 	uint8_t reg = *((uint8_t*)a);
+
+    // Configuration for encoder port 0
+    if ((reg & B_EN_ENCODER_PORT0) && !_states_.quad_counter.port0)
+    {
+	    _states_.quad_counter.port0 = true;
+	    
+	    /* Turn off interrupts on the Encoder pins and redefine pins to input */
+	    PORTD_INTCTRL &= ~(PORT_INT0LVL_gm);                                // Shut down interrupts on PORTD
+	    io_pin2in(&PORTD, 4, PULL_IO_TRISTATE, SENSE_IO_LOW_LEVEL);         // POKE0_IR
+	    io_pin2in(&PORTD, 5, PULL_IO_TRISTATE, SENSE_IO_LOW_LEVEL);         // POKE0_IO
+	    
+	    /* Set up quadrature decoding event */
+	    EVSYS_CH0MUX = EVSYS_CHMUX_PORTD_PIN4_gc;                           // P. 77
+	    EVSYS_CH0CTRL = EVSYS_QDEN_bm | EVSYS_DIGFILT_2SAMPLES_gc;          // P. 78
+	    
+	    /* Stop and reset timer */
+	    TCD1_CTRLA = TC_CLKSEL_OFF_gc;
+	    TCD1_CTRLFSET = TC_CMD_RESET_gc;
+	    
+	    /* Configure timer */
+	    TCD1_CTRLD = TC_EVACT_QDEC_gc | TC_EVSEL_CH0_gc;	                // P. 180-1
+	    TCD1_PER = 0xFFFF;
+	    TCD1_CNT = 0x8000;
+	    previous_encoder_poke0 = 0x8000;
+	    
+	    /* Start timer */
+	    TCD1_CTRLA=TC_CLKSEL_DIV1_gc;
+    }
     
-    if ((reg & B_EN_ENCODER_PORT2) && !_states_.quad_counter.port2)
+    if (!(reg & B_EN_ENCODER_PORT0) && _states_.quad_counter.port0)
+    {
+	    if (_states_.quad_counter.port0)
+	    {
+		    _states_.quad_counter.port0 = false;
+		    
+		    /* Stop and reset timer */
+		    TCD1_CTRLA = TC_CLKSEL_OFF_gc;
+		    TCD1_CTRLFSET = TC_CMD_RESET_gc;
+		    
+		    /* Turn inputs to default configuration (same as *init_ios()* func)  */
+		    io_pin2in(&PORTD, 4, PULL_IO_UP, SENSE_IO_EDGES_BOTH);  // POKE1_IR
+		    io_set_int(&PORTD, INT_LEVEL_LOW, 0, (3<<4), false);    // POKE1_IR  & IO IN
+		    
+		    /* Reset register */
+		    app_regs.REG_DATA_ENCODERS[REG_DATA_ENCODERS_INDEX_ENCODER0] = 0;
+	    }
+    }
+	
+    // Configuration for encoder port 1
+    if ((reg & B_EN_ENCODER_PORT1) && !_states_.quad_counter.port1)
     {        
-        _states_.quad_counter.port2 = true;
+        _states_.quad_counter.port1 = true;
         
         /* Turn off interrupts on the Encoder pins and redefine pins to input */
-        PORTF_INTCTRL &= ~(PORT_INT0LVL_gm);                                // Shut down interrupts on PORTF
-        PORTF_INTCTRL &= ~(PORT_INT0LVL_gm);                                // Shut down interrupts on PORTF
-        io_pin2in(&PORTF, 4, PULL_IO_TRISTATE, SENSE_IO_LOW_LEVEL);         // POKE2_IR
-        io_pin2in(&PORTF, 5, PULL_IO_TRISTATE, SENSE_IO_LOW_LEVEL);         // POKE2_IO
+        PORTE_INTCTRL &= ~(PORT_INT0LVL_gm);                                // Shut down interrupts on PORTE
+        io_pin2in(&PORTE, 4, PULL_IO_TRISTATE, SENSE_IO_LOW_LEVEL);         // POKE1_IR
+        io_pin2in(&PORTE, 5, PULL_IO_TRISTATE, SENSE_IO_LOW_LEVEL);         // POKE1_IO
         
         /* Set up quadrature decoding event */
-        EVSYS_CH0MUX = EVSYS_CHMUX_PORTF_PIN4_gc;                           // P. 77
-        EVSYS_CH0CTRL = EVSYS_QDEN_bm | EVSYS_DIGFILT_2SAMPLES_gc;          // P. 78
+		EVSYS_CH4MUX = EVSYS_CHMUX_PORTE_PIN4_gc;                           // P. 77
+		EVSYS_CH4CTRL = EVSYS_QDEN_bm | EVSYS_DIGFILT_2SAMPLES_gc;          // P. 78		
         
         /* Stop and reset timer */
-        TCD1_CTRLA = TC_CLKSEL_OFF_gc;
-        TCD1_CTRLFSET = TC_CMD_RESET_gc;
-        
+		TCE1_CTRLA = TC_CLKSEL_OFF_gc;
+		TCE1_CTRLFSET = TC_CMD_RESET_gc;
+		        
         /* Configure timer */
-        TCD1_CTRLD = TC_EVACT_QDEC_gc | TC_EVSEL_CH0_gc;	                // P. 180-1
-        TCD1_PER = 0xFFFF;
-        TCD1_CNT = 0x8000;
-		  previous_encoder_poke2 = 0x8000;
+		TCE1_CTRLD = TC_EVACT_QDEC_gc | TC_EVSEL_CH4_gc;	                // P. 180-1
+		TCE1_PER = 0xFFFF;
+		TCE1_CNT = 0x8000;
+		previous_encoder_poke1 = 0x8000;
         
         /* Start timer */
-        TCD1_CTRLA=TC_CLKSEL_DIV1_gc;
+    	TCE1_CTRLA=TC_CLKSEL_DIV1_gc;
     }
     
-    if (!(reg & B_EN_ENCODER_PORT2) && _states_.quad_counter.port2)
+    if (!(reg & B_EN_ENCODER_PORT1) && _states_.quad_counter.port1)
     {
-        if (_states_.quad_counter.port2)
+        if (_states_.quad_counter.port1)
         {
-            _states_.quad_counter.port2 = false;
+            _states_.quad_counter.port1 = false;
             
             /* Stop and reset timer */
-            TCD1_CTRLA = TC_CLKSEL_OFF_gc;
-            TCD1_CTRLFSET = TC_CMD_RESET_gc;
+			TCE1_CTRLA = TC_CLKSEL_OFF_gc;
+			TCE1_CTRLFSET = TC_CMD_RESET_gc;
         
             /* Turn inputs to default configuration (same as *init_ios()* func)  */
-            io_pin2in(&PORTF, 4, PULL_IO_UP, SENSE_IO_EDGES_BOTH);  // POKE2_IR
-            io_set_int(&PORTF, INT_LEVEL_LOW, 0, (3<<4), false);    // POKE2_IR  & IO IN
+            io_pin2in(&PORTE, 4, PULL_IO_UP, SENSE_IO_EDGES_BOTH);  // POKE1_IR
+            io_set_int(&PORTE, INT_LEVEL_LOW, 0, (3<<4), false);    // POKE1_IR  & IO IN
 		
             /* Reset register */
-            app_regs.REG_DATA[1] = 0;
+            app_regs.REG_DATA_ENCODERS[REG_DATA_ENCODERS_INDEX_ENCODER1] = 0;
         }
     }
+
+    // Configuration for encoder port 2
+    if ((reg & B_EN_ENCODER_PORT2) && !_states_.quad_counter.port2)
+    {
+	    _states_.quad_counter.port2 = true;
+	    
+	    /* Turn off interrupts on the Encoder pins and redefine pins to input */
+	    PORTF_INTCTRL &= ~(PORT_INT0LVL_gm);                                // Shut down interrupts on PORTF
+	    io_pin2in(&PORTF, 4, PULL_IO_TRISTATE, SENSE_IO_LOW_LEVEL);         // POKE2_IR
+	    io_pin2in(&PORTF, 5, PULL_IO_TRISTATE, SENSE_IO_LOW_LEVEL);         // POKE2_IO
+	    
+	    /* Set up quadrature decoding event */
+	    EVSYS_CH2MUX = EVSYS_CHMUX_PORTF_PIN4_gc;                           // P. 77
+	    EVSYS_CH2CTRL = EVSYS_QDEN_bm | EVSYS_DIGFILT_2SAMPLES_gc;          // P. 78
+	    
+	    /* Stop and reset timer */
+	    TCF1_CTRLA = TC_CLKSEL_OFF_gc;
+	    TCF1_CTRLFSET = TC_CMD_RESET_gc;
+	    
+	    /* Configure timer */
+	    TCF1_CTRLD = TC_EVACT_QDEC_gc | TC_EVSEL_CH2_gc;	                // P. 180-1
+	    TCF1_PER = 0xFFFF;
+	    TCF1_CNT = 0x8000;
+	    previous_encoder_poke2 = 0x8000;
+	    
+	    /* Start timer */
+	    TCF1_CTRLA=TC_CLKSEL_DIV1_gc;
+    }
     
+    if (!(reg & B_EN_ENCODER_PORT2) && _states_.quad_counter.port2)
+    {
+	    if (_states_.quad_counter.port2)
+	    {
+		    _states_.quad_counter.port2 = false;
+		    
+		    /* Stop and reset timer */
+		    TCF1_CTRLA = TC_CLKSEL_OFF_gc;
+		    TCF1_CTRLFSET = TC_CMD_RESET_gc;
+		    
+		    /* Turn inputs to default configuration (same as *init_ios()* func)  */
+		    io_pin2in(&PORTF, 4, PULL_IO_UP, SENSE_IO_EDGES_BOTH);  // POKE2_IR
+		    io_set_int(&PORTF, INT_LEVEL_LOW, 0, (3<<4), false);    // POKE2_IR  & IO IN
+		    
+		    /* Reset register */
+		    app_regs.REG_DATA_ENCODERS[REG_DATA_ENCODERS_INDEX_ENCODER2] = 0;
+	    }
+    }
+	
     app_regs.REG_EN_ENCODERS = reg;
 	return true;
 }
@@ -1551,10 +1646,36 @@ bool app_write_REG_CONF_ENCODERS(void *a)
 	return true;
 }
 /************************************************************************/
-/* REG_RESERVED2                                                        */
+/* REG_DATA_ENCODERS                                                      */
 /************************************************************************/
-void app_read_REG_RESERVED2(void) {}
-bool app_write_REG_RESERVED2(void *a) {return true;}
+void app_read_REG_DATA_ENCODERS(void) {}
+bool app_write_REG_DATA_ENCODERS(void *a) 
+{
+	uint16_t *reg = ((uint16_t*)a);
+
+	// Write the new values for the encoders only
+	app_regs.REG_DATA_ENCODERS[REG_DATA_ENCODERS_INDEX_ENCODER0] = reg[REG_DATA_ENCODERS_INDEX_ENCODER0];
+	app_regs.REG_DATA_ENCODERS[REG_DATA_ENCODERS_INDEX_ENCODER1] = reg[REG_DATA_ENCODERS_INDEX_ENCODER1];
+	app_regs.REG_DATA_ENCODERS[REG_DATA_ENCODERS_INDEX_ENCODER2] = reg[REG_DATA_ENCODERS_INDEX_ENCODER2];
+
+	// Now update the counters for the encoders that are active
+	if (_states_.quad_counter.port0)
+	{
+		TCD1_CNT = 0x8000 + reg[REG_DATA_ENCODERS_INDEX_ENCODER0];
+	}
+	if (_states_.quad_counter.port1)
+	{
+		TCE1_CNT = 0x8000 + reg[REG_DATA_ENCODERS_INDEX_ENCODER1];
+	}
+	if (_states_.quad_counter.port2)
+	{
+		TCF1_CNT = 0x8000 + reg[REG_DATA_ENCODERS_INDEX_ENCODER2];
+	}
+	
+	return true;
+}
+	
+	
 /************************************************************************/
 /* REG_RESERVED3                                                        */
 /************************************************************************/
@@ -1759,22 +1880,39 @@ bool app_write_REG_RESERVED17(void *a) {return true;}
 
 
 /************************************************************************/
-/* REG_ENCODER_PORT2_RESET                                              */
+/* REG_ENCODERS_RESET                                                    */
 /************************************************************************/
 void app_read_REG_ENCODERS_RESET(void) {}
 bool app_write_REG_ENCODERS_RESET(void *a)
 {
 	uint8_t reg = *((uint8_t*)a);
     
-    if (reg & B_RST_ENCODER_PORT2)
+    if (reg & B_RST_ENCODER_PORT0)
     {
-        if (_states_.quad_counter.port2)
+        if (_states_.quad_counter.port0)
         {
             TCD1_CNT = 0x8000;
-				previous_encoder_poke2 = 0x8000;
+			previous_encoder_poke0 = 0x8000;
         }
     }
 
+    if (reg & B_RST_ENCODER_PORT1)
+    {
+	    if (_states_.quad_counter.port1)
+	    {
+		    TCE1_CNT = 0x8000;
+		    previous_encoder_poke1 = 0x8000;
+	    }
+    }
+    if (reg & B_RST_ENCODER_PORT2)
+    {
+	    if (_states_.quad_counter.port2)
+	    {
+		    TCF1_CNT = 0x8000;
+		    previous_encoder_poke2 = 0x8000;
+	    }
+    }
+
 	app_regs.REG_ENCODERS_RESET = reg;
 	return true;
 }
diff --git a/Firmware/Behavior/app_funcs.h b/Firmware/Behavior/app_funcs.h
index 5d57c95..be6153a 100644
--- a/Firmware/Behavior/app_funcs.h
+++ b/Firmware/Behavior/app_funcs.h
@@ -72,7 +72,7 @@ void app_read_REG_EN_SERVOS(void);
 void app_read_REG_DIS_SERVOS(void);
 void app_read_REG_EN_ENCODERS(void);
 void app_read_REG_CONF_ENCODERS(void);
-void app_read_REG_RESERVED2(void);
+void app_read_REG_DATA_ENCODERS(void);
 void app_read_REG_RESERVED3(void);
 void app_read_REG_RESERVED4(void);
 void app_read_REG_RESERVED5(void);
@@ -165,7 +165,7 @@ bool app_write_REG_EN_SERVOS(void *a);
 bool app_write_REG_DIS_SERVOS(void *a);
 bool app_write_REG_EN_ENCODERS(void *a);
 bool app_write_REG_CONF_ENCODERS(void *a);
-bool app_write_REG_RESERVED2(void *a);
+bool app_write_REG_DATA_ENCODERS(void *a);
 bool app_write_REG_RESERVED3(void *a);
 bool app_write_REG_RESERVED4(void *a);
 bool app_write_REG_RESERVED5(void *a);
diff --git a/Firmware/Behavior/app_ios_and_regs.c b/Firmware/Behavior/app_ios_and_regs.c
index 559c824..5b8755d 100644
--- a/Firmware/Behavior/app_ios_and_regs.c
+++ b/Firmware/Behavior/app_ios_and_regs.c
@@ -133,7 +133,7 @@ uint8_t app_regs_type[] = {
 	TYPE_U8,
 	TYPE_U8,
 	TYPE_U8,
-	TYPE_U8,
+	TYPE_I16,
 	TYPE_U8,
 	TYPE_U8,
 	TYPE_U8,
@@ -187,7 +187,7 @@ uint16_t app_regs_n_elements[] = {
 	1,
 	1,
 	1,
-	3,
+	2,
 	1,
 	1,
 	1,
@@ -227,7 +227,7 @@ uint16_t app_regs_n_elements[] = {
 	1,
 	1,
 	1,
-	1,
+	3,
 	1,
 	1,
 	1,
@@ -321,7 +321,7 @@ uint8_t *app_regs_pointer[] = {
 	(uint8_t*)(&app_regs.REG_DIS_SERVOS),
 	(uint8_t*)(&app_regs.REG_EN_ENCODERS),
 	(uint8_t*)(&app_regs.REG_CONF_ENCODERS),
-	(uint8_t*)(&app_regs.REG_RESERVED2),
+	(uint8_t*)(&app_regs.REG_DATA_ENCODERS),
 	(uint8_t*)(&app_regs.REG_RESERVED3),
 	(uint8_t*)(&app_regs.REG_RESERVED4),
 	(uint8_t*)(&app_regs.REG_RESERVED5),
diff --git a/Firmware/Behavior/app_ios_and_regs.h b/Firmware/Behavior/app_ios_and_regs.h
index 2fcd64c..31449bb 100644
--- a/Firmware/Behavior/app_ios_and_regs.h
+++ b/Firmware/Behavior/app_ios_and_regs.h
@@ -141,7 +141,7 @@ typedef struct
 	uint8_t REG_PORT_DIOS_OUT;
 	uint8_t REG_PORT_DIOS_CONF;
 	uint8_t REG_PORT_DIOS_IN;
-	int16_t REG_DATA[3];
+	int16_t REG_DATA[2];
 	uint16_t REG_OUTPUT_PULSE_EN;
 	uint16_t REG_PULSE_PORT0_DO;
 	uint16_t REG_PULSE_PORT1_DO;
@@ -181,7 +181,7 @@ typedef struct
 	uint8_t REG_DIS_SERVOS;
 	uint8_t REG_EN_ENCODERS;
 	uint8_t REG_CONF_ENCODERS;
-	uint8_t REG_RESERVED2;
+	int16_t REG_DATA_ENCODERS[3];
 	uint8_t REG_RESERVED3;
 	uint8_t REG_RESERVED4;
 	uint8_t REG_RESERVED5;
@@ -238,7 +238,9 @@ typedef struct
 #define ADD_REG_PORT_DIOS_OUT               41 // U8     Control the correspondent DIO
 #define ADD_REG_PORT_DIOS_CONF              42 // U8     Set the DIOs direction (1 is output)
 #define ADD_REG_PORT_DIOS_IN                43 // U8     State of the DIOs
-#define ADD_REG_DATA                        44 // I16    Voltage at ADC input and decoder (poke 2) value
+#define ADD_REG_DATA                        44 // I16[2] Voltage at ADC input and decoder (poke 2) value
+#define REG_DATA_INDEX_ADC0						0	// Index for the value of ADC0
+#define REG_DATA_INDEX_ADC1						1   // Index for the value of ADC1
 #define ADD_REG_OUTPUT_PULSE_EN             45 // U16    Enable the output pulse for the selected output
 #define ADD_REG_PULSE_PORT0_DO              46 // U16    Configuration of the output pulse [1 : 65535]
 #define ADD_REG_PULSE_PORT1_DO              47 // U16    
@@ -278,7 +280,10 @@ typedef struct
 #define ADD_REG_DIS_SERVOS                  81 // U8     Disable servo motors control
 #define ADD_REG_EN_ENCODERS                 82 // U8     Enable encoders
 #define ADD_REG_CONF_ENCODERS               83 // U8     Configure the operation mode of the quadrature encoders
-#define ADD_REG_RESERVED2                   84 // U8     Reserved for future use
+#define ADD_REG_DATA_ENCODERS               84 // I16[3] Current position of the encoders
+#define REG_DATA_ENCODERS_INDEX_ENCODER0		0 // Index for the value of Encoder 0
+#define REG_DATA_ENCODERS_INDEX_ENCODER1		1 // Index for the value of Encoder 1
+#define REG_DATA_ENCODERS_INDEX_ENCODER2		2 // Index for the value of Encoder 2
 #define ADD_REG_RESERVED3                   85 // U8     Reserved for future use
 #define ADD_REG_RESERVED4                   86 // U8     Reserved for future use
 #define ADD_REG_RESERVED5                   87 // U8     Reserved for future use
@@ -327,7 +332,7 @@ typedef struct
 /* Memory limits */
 #define APP_REGS_ADD_MIN                    0x20
 #define APP_REGS_ADD_MAX                    0x7A
-#define APP_NBYTES_OF_REG_BANK              134
+#define APP_NBYTES_OF_REG_BANK              138
 
 /************************************************************************/
 /* Registers' bits                                                      */
@@ -366,11 +371,15 @@ typedef struct
 #define B_EN_CAM_OUT1                      (1<<1)       // Camera on digital output 1
 #define B_EN_SERVO_OUT2                    (1<<2)       // Servo on digital output 2
 #define B_EN_SERVO_OUT3                    (1<<3)       // Servo on digital output 3
+#define B_EN_ENCODER_PORT0                 (1<<0)       // Encoder on port 0
+#define B_EN_ENCODER_PORT1                 (1<<1)       // Encoder on port 1
 #define B_EN_ENCODER_PORT2                 (1<<2)       // Encoder on port 2
 #define MSK_ENCODERS_MODE                  0x01         // 
 #define GM_POSITION                        0x00         // 
 #define GM_DISPLACEMENT                    0x01         // 
 #define B_CAM_ACQ                          (1<<0)       // Camera frame was triggered
+#define B_RST_ENCODER_PORT0                (1<<0)       // Reset the encoder counter on Port 0
+#define B_RST_ENCODER_PORT1                (1<<1)       // Reset the encoder counter on Port 1
 #define B_RST_ENCODER_PORT2                (1<<2)       // Reset the encoder counter on Port 2
 #define B_EN_SRL_TSTAMP_PORT2              (1<<2)       // Enable the serial timestamp TX on Port 2
 #define MSK_MIMIC                          0x0F         // 
diff --git a/Firmware/Behavior/interrupts.c b/Firmware/Behavior/interrupts.c
index ff83cc1..0aa0cd5 100644
--- a/Firmware/Behavior/interrupts.c
+++ b/Firmware/Behavior/interrupts.c
@@ -325,9 +325,9 @@ ISR(ADCA_CH0_vect, ISR_NAKED)
 	if (first_adc_channel)
 	{
 		first_adc_channel = false;
-		
+				
 		/* Read ADC0 Channel 0 */
-		app_regs.REG_DATA[0] = ((int16_t)(ADCA_CH0_RES & 0x0FFF)) - AdcOffset;
+		app_regs.REG_DATA[REG_DATA_INDEX_ADC0] = ((int16_t)(ADCA_CH0_RES & 0x0FFF)) - AdcOffset;
 		
 		if (read_ADC1_AVAILABLE)
 		{
@@ -343,14 +343,14 @@ ISR(ADCA_CH0_vect, ISR_NAKED)
 	else
 	{		
 		/* Read ADC0 Channel 2 */
-		app_regs.REG_DATA[2] = ((int16_t)(ADCA_CH0_RES & 0x0FFF)) - AdcOffset;
+		app_regs.REG_DATA[REG_DATA_INDEX_ADC1] = ((int16_t)(ADCA_CH0_RES & 0x0FFF)) - AdcOffset;
 		
 		/* Validate readings */
-		if (app_regs.REG_DATA[0] < 0)
-			app_regs.REG_DATA[0] = 0;			
-		if (app_regs.REG_DATA[2] < 0)
-			app_regs.REG_DATA[2] = 0;
-			
+		if (app_regs.REG_DATA[REG_DATA_INDEX_ADC0] < 0)
+			app_regs.REG_DATA[REG_DATA_INDEX_ADC0] = 0;			
+		if (app_regs.REG_DATA[REG_DATA_INDEX_ADC1] < 0)
+			app_regs.REG_DATA[REG_DATA_INDEX_ADC1] = 0;
+		
 		send_event = true;
 	}
 	
diff --git a/device.yml b/device.yml
index faec537..7581c2d 100644
--- a/device.yml
+++ b/device.yml
@@ -3,7 +3,7 @@
 # yaml-language-server: $schema=https://harp-tech.org/draft-02/schema/device.json
 device: Behavior
 whoAmI: 1216
-firmwareVersion: "3.2"
+firmwareVersion: "3.3"
 hardwareTargets: "1.1"
 registers:
   DigitalInputState:
@@ -66,18 +66,15 @@ registers:
   AnalogData:
     address: 44
     type: S16
-    length: 3
+    length: 2
     access: Event
-    description: Voltage at the ADC input and encoder value on Port 2
+    description: Voltage at the ADC inputs and encoder values
     payloadSpec:
       AnalogInput0:
         offset: 0
         description: The voltage at the output of the ADC channel 0.
-      Encoder:
-        offset: 1
-        description: The quadrature counter value on Port 2
       AnalogInput1:
-        offset: 2
+        offset: 1
         description: The voltage at the output of the ADC channel 1.
   OutputPulseEnable:
     <<: *output
@@ -282,9 +279,22 @@ registers:
     access: Write
     maskType: EncoderModeConfig
     description: Configures the operation mode of the quadrature encoders.
-  Reserved2:
-    <<: *reserved
+  EncoderData:
     address: 84
+    type: S16
+    length: 3
+    access: Event
+    description: Current values for the active encoders
+    payloadSpec:
+      Encoder0:
+        offset: 0
+        description: The quadrature counter value on Port 0
+      Encoder1:
+        offset: 1
+        description: The quadrature counter value on Port 1
+      Encoder2:
+        offset: 2
+        description: The quadrature counter value on Port 2
   Reserved3:
     <<: *reserved
     address: 85
@@ -433,6 +443,7 @@ registers:
     type: U8
     access: Write
     description: Specifies the low pass filter time value for poke inputs, in ms.
+
 bitMasks:
   DigitalInputs:
     description: Specifies the state of port digital input lines.
@@ -488,10 +499,12 @@ bitMasks:
     description: Specifies servo output enable bits.
     bits:
       ServoOutput2: 0x4
-      ServoOutput3: 0x8
+      ServoOutput3: 0x8 
   EncoderInputs:
     description: Specifies quadrature counter enable bits.
     bits:
+      EncoderPort0: 0x1
+      EncoderPort1: 0x2
       EncoderPort2: 0x4
   FrameAcquired:
     description: Specifies that camera frame was acquired.