diff --git a/adc.c b/adc.cpp
similarity index 100%
rename from adc.c
rename to adc.cpp
diff --git a/avr.c b/avr.cpp
similarity index 54%
rename from avr.c
rename to avr.cpp
index ac303d0..a5194a4 100644
--- a/avr.c
+++ b/avr.cpp
@@ -1,4 +1,5 @@
 #include "avr.h"
+#include "adc.h"
 #include "util.h"
 
 void setMotorSpeed(MOTOR *motor, int speed) {
@@ -14,3 +15,20 @@ void setMotorSpeed(MOTOR *motor, int speed) {
   
   *motor->pwmPort = CLAMP(0, ABS(speed), 255);
 }
+
+int getButton() {
+  uint16_t val = getADCValue(7);
+  
+  if(val < 144) return 5;
+  if(val < 228) return 4;
+  if(val < 304) return 3;
+  if(val < 376) return 2;
+  if(val < 600) return 1;
+  
+  return 0;
+}
+
+void waitForButton(int i) {
+  while(getButton() != CLAMP(0, i, 5));
+  while(getButton() != 0);
+}
diff --git a/avr.h b/avr.h
index 3727bdf..2d13616 100644
--- a/avr.h
+++ b/avr.h
@@ -13,4 +13,7 @@ typedef struct {
 
 void setMotorSpeed(MOTOR *motor, int speed);
 
+int getButton();
+void waitForButton(int i);
+
 #endif
diff --git a/hardware.c b/hardware.c
deleted file mode 100644
index 6bc329e..0000000
--- a/hardware.c
+++ /dev/null
@@ -1,6 +0,0 @@
-#include "hardware.h"
-
-
-void initHardware() {
-  
-}
diff --git a/hardware.cpp b/hardware.cpp
new file mode 100644
index 0000000..41c7ea2
--- /dev/null
+++ b/hardware.cpp
@@ -0,0 +1,28 @@
+#include "hardware.h"
+#include "adc.h"
+#include "i2c.h"
+
+
+void initHardware() {
+  DDRA = 0x00;
+  PORTA = 0xFF;
+  
+  DDRB = 0xFF;
+  PORTB = 0x00;
+  
+  DDRC = 0x7C;
+  PORTC = 0x83;
+  
+  DDRD = 0xFF;
+  PORTC = 0x00;
+  
+  TCCR0 = 0x62;
+  TCCR1A = 0xA1;
+  TCCR1B = 0x82;
+  OCR0 = 0;
+  OCR1A = 0;
+  OCR1B = 0;
+  
+  initADC();
+  initI2C();
+}
diff --git a/i2c.cpp b/i2c.cpp
new file mode 100644
index 0000000..61450fa
--- /dev/null
+++ b/i2c.cpp
@@ -0,0 +1,148 @@
+#include "i2c.h"
+
+#include <util/twi.h>
+
+
+static bool I2CStartSend(uint8_t addr);
+static bool I2CStartRecv(uint8_t addr);
+static void I2CStop();
+
+
+static bool I2CStartSend(uint8_t addr) {
+  TWCR = (1<<TWEN)|(1<<TWINT)|(1<<TWSTA);
+  
+  while(!(TWCR & (1<<TWINT)));
+      
+  if (TW_STATUS != TW_START) return false;
+  
+  TWDR = (addr&0xFE);
+  TWCR = (1<<TWEN)|(1<<TWINT);
+  
+  while(!(TWCR & (1<<TWINT)));
+  
+  if(TW_STATUS == TW_MT_SLA_ACK)
+    return true;
+  
+  I2CStop();
+  return false;
+}
+
+static bool I2CStartRecv(uint8_t addr) {
+  TWCR = (1<<TWEN)|(1<<TWINT)|(1<<TWSTA);
+  
+  while(!(TWCR & (1<<TWINT)));
+      
+  if (TW_STATUS != TW_START) return false;
+  
+  TWDR = (addr|1);
+  TWCR = (1<<TWEN)|(1<<TWINT);
+  
+  while(!(TWCR & (1<<TWINT)));
+  
+  if(TW_STATUS == TW_MR_SLA_ACK)
+    return true;
+  
+  I2CStop();
+  return false;
+}
+
+static void I2CStop() {
+  TWCR = (1<<TWINT)|(1<<TWEN)|(1<<TWSTO);
+}
+
+
+void initI2C() {
+  TWBR = 10;
+  TWCR = (1<<TWEN);
+  TWSR = 0;
+}
+
+bool I2CSendByte(uint8_t addr, uint8_t data) {
+  if(!I2CStartSend(addr)) return false;
+  
+  TWDR=data;
+  
+  TWCR = (1<<TWEN)|(1<<TWINT);
+  while(!(TWCR & (1<<TWINT)));
+  
+  if(TW_STATUS != TW_MT_DATA_ACK) {
+    I2CStop();
+    return false;
+  }
+  
+  I2CStop();
+  return true;
+}
+
+bool I2CSend(uint8_t addr, uint8_t *data, int length) {
+  if(!I2CStartSend(addr)) return false;
+  
+  for(int i = 0; i < length; i++) {
+    TWDR=data[i];
+  
+    TWCR = (1<<TWEN)|(1<<TWINT);
+    while(!(TWCR & (1<<TWINT)));
+    
+    if(TW_STATUS != TW_MT_DATA_ACK) {
+      I2CStop();
+      return false;
+    }
+  }
+  
+  I2CStop();
+  return true;
+}
+
+bool I2CRecvByte(uint8_t addr, uint8_t *data) {
+  if(!I2CStartRecv(addr)) return false;
+  
+  while(!(TWCR & (1<<TWINT)));
+  
+  if(TW_STATUS != TW_MR_DATA_ACK && TW_STATUS != TW_MR_DATA_NACK) {
+    I2CStop();
+    return false;
+  }
+  
+  *data = TWDR;
+  TWCR = (1<<TWEN)|(1<<TWINT);
+  
+  I2CStop();
+  return true;
+}
+
+int I2CRecv(uint8_t addr, uint8_t *data, int length) {
+  if(!I2CStartRecv(addr)) return -1;
+  
+  for(int i = 0; i < length-1; i++) {
+    while(!(TWCR & (1<<TWINT)));
+    
+    switch(TW_STATUS) {
+      case TW_MR_DATA_ACK:
+        data[i] = TWDR;
+        TWCR = (1<<TWEN)|(1<<TWINT)|(1<<TWEA);
+        break;
+      case TW_MR_DATA_NACK:
+        data[i] = TWDR;
+        TWCR = (1<<TWEN)|(1<<TWINT);
+        I2CStop();
+        return i+1;
+      default:
+        I2CStop();
+        return -1;
+    }
+  }
+  
+  while(!(TWCR & (1<<TWINT)));
+  
+  switch(TW_STATUS) {
+    case TW_MR_DATA_ACK:
+    case TW_MR_DATA_NACK:
+      data[length-1] = TWDR;
+      TWCR = (1<<TWEN)|(1<<TWINT);
+      I2CStop();
+      return length;
+  }
+  
+  I2CStop();
+  return -1;
+}
diff --git a/i2c.h b/i2c.h
new file mode 100644
index 0000000..4f6a5b7
--- /dev/null
+++ b/i2c.h
@@ -0,0 +1,13 @@
+#ifndef _ROBOCUP_I2C_H_
+#define _ROBOCUP_I2C_H_
+
+#include <stdint.h>
+
+
+void initI2C();
+bool I2CSendByte(uint8_t addr, uint8_t data);
+bool I2CSend(uint8_t addr, uint8_t *data, int length);
+bool I2CRecvByte(uint8_t addr, uint8_t *data);
+int I2CRecv(uint8_t addr, uint8_t *data, int length);
+
+#endif
diff --git a/main.cpp b/main.cpp
new file mode 100644
index 0000000..2a3f761
--- /dev/null
+++ b/main.cpp
@@ -0,0 +1,5 @@
+int main() {
+  
+  
+  return 0;
+}