diff --git a/Control/firmware/uart_echo/main.c b/Control/firmware/uart_echo/main.c
index 37dbcbf..c2bb0c7 100755
--- a/Control/firmware/uart_echo/main.c
+++ b/Control/firmware/uart_echo/main.c
@@ -288,17 +288,6 @@ int adc_is_fininshed()
 	return (int)(0 == (ADCSRA & (1 << ADSC)));
 }
 
-int16_t adc_read()
-{
-	return adc_value;
-	
-	while(!adc_is_fininshed());
-	uint16_t value_low = (uint16_t)ADCL;
-	uint16_t value_high = (uint16_t)ADCH;
-	ADCSRA |= (1<<ADIF);
-	return value_high << 8 | value_low;
-}
-
 void uart_putc(char c)
 {
 	while((UCSR0A & (1<<UDRE0)) == 0)
@@ -338,6 +327,33 @@ void port_init()
 }
 FILE uart_file = FDEV_SETUP_STREAM(uart_putchar, NULL, _FDEV_SETUP_WRITE);
 
+enum MCP47x6_flags
+{
+	C0 = 0x20,
+	C1 = 0x40,
+	C2 = 0x80,
+	PDN0 = 0x10,
+	PDN1 = 0x20,
+	mask = 0xF0
+};
+
+size_t MCP47x6_write_volatile_dac(uint16_t dac_value, uint8_t *buffer, size_t size)
+{
+	*(buffer++) = ~mask & (uint8_t)(dac_value >> 8);
+	*(buffer++) = (uint8_t)(dac_value >> 0);
+	
+	return 2;
+}
+
+size_t MCP47x6_write_volatile_mem(uint16_t dac_value, uint8_t *buffer, size_t size)
+{
+	*(buffer++) = mask & 0x40;
+	dac_value <<= 4;
+	*(buffer++) = (uint8_t)(dac_value >> 8);
+	*(buffer++) = (uint8_t)(dac_value >> 0);
+	
+	return 3;
+}
 
 int main(void)
 {
@@ -352,8 +368,9 @@ int main(void)
 	stdout = &uart_file;
 	printf("UART test version 1.0\n\n");
 
-	uint8_t write_dac_full[] = {0x40, 0x00, 0x00};		// 6.1 Write Volatile DAC Register (C2:C0 = ‘00x’)
-	i2c_send(0x60, I2C_WRITE, write_dac_full, sizeof(write_dac_full));
+	uint8_t dac_cmd[] = {0x00, 0x00, 0x00};		// 6.1 Write Volatile DAC Register (C2:C0 = ‘00x’)
+	size_t res = MCP47x6_write_volatile_mem(0x0000, dac_cmd, sizeof(dac_cmd));
+	i2c_send(0x60, I2C_WRITE, dac_cmd, res);
 
 	sei();
 	adc_start_conversion(adc_ch);
@@ -369,9 +386,12 @@ int main(void)
 			switch(msg.code)
 			{
 				case Command:
-				printf("Command : %c\n", msg.data);
-				uint8_t write_dac[] = {0x00+(msg.data-'0'), 0x00};					// 6.2 Write Volatile Memory (C2:C0 = ‘010’)
-				i2c_send(0x60, I2C_WRITE, write_dac, sizeof(write_dac));
+				{
+					printf("Command : %c\n", msg.data);
+					uint8_t dac_cmd[] = {0x00, 0x00};					// 6.2 Write Volatile Memory (C2:C0 = ‘010’)
+					size_t res = MCP47x6_write_volatile_dac((uint16_t)(msg.data-'0')<<8, dac_cmd, sizeof(dac_cmd));
+					i2c_send(0x60, I2C_WRITE, dac_cmd, res);
+				}
 				break;
 			
 				case AdcComplete:
@@ -380,6 +400,9 @@ int main(void)
 					if (ch == 6)
 					{
 						printf("ADC=%d\n", adc_value[ch]);
+						uint8_t dac_cmd[] = {0x00, 0x00};					// 6.2 Write Volatile Memory (C2:C0 = ‘010’)
+						size_t res = MCP47x6_write_volatile_dac(adc_value[ch]<<2, dac_cmd, sizeof(dac_cmd));
+						i2c_send(0x60, I2C_WRITE, dac_cmd, res);
 					}
 				}
 				break;