diff --git a/src/dac.cpp b/src/dac.cpp
index 25fb628..4fe9265 100644
--- a/src/dac.cpp
+++ b/src/dac.cpp
@@ -1,127 +1,74 @@
-#include <avr/io.h>
-#include <inttypes.h>
-#include <HardwareSerial.h>
-
 #include "util.h"
 #include "config.h"
 #include "dac.h"
-#include "i2cmaster.h"
-
-#include <Wire.h>
-
-#include "Arduino.h"
 
 MCP4725 xDac;
 MCP4725 yDac;
 
 Dac xyz;
 
-void Dac::Setup(void)
-{
-  xDac.begin(0x61);
-  yDac.begin(0x60);
-  Wire.begin();
-  //i2c_init();
+int i2c_init() {
+
+	if (!bcm2835_init())
+		return (1);
+
+	bcm2835_i2c_set_baudrate(3400000);
+
+	return (0);
+}
+
+void Dac::Setup(void) {
+	xDac.begin(0x61);
+	yDac.begin(0x60);
+
+	i2c_init();
 
 }
 
 void Dac::SetXY(int dacX, int dacY) {
-  //xDac.setVoltageFast(dacX * 3, false);
-  //yDac.setVoltageFast(dacY * 3, false);
+	//xDac.setVoltageFast(dacX * 3, false);
+	//yDac.setVoltageFast(dacY * 3, false);
 
-  //Fast
-  xDac.setVoltageFast(dacX * 3);
-  yDac.setVoltageFast(dacY * 3);
+	//Fast
+	xDac.setVoltageFast(dacX * 3);
+	yDac.setVoltageFast(dacY * 3);
 }
 
 void Dac::SetZ(uint8_t z) {
-  PORT_ZDAC = (z) ? (PORT_ZDAC & ~ZDAC_BV) : (PORT_ZDAC | ZDAC_BV);
+	PORT_ZDAC = (z) ? (PORT_ZDAC & ~ZDAC_BV) : (PORT_ZDAC | ZDAC_BV);
 }
 
 MCP4725::MCP4725() {
 }
 
-
 void MCP4725::begin(uint8_t addr) {
-  _i2caddr = addr;
+	_i2caddr = addr;
 }
 
-/**************************************************************************/
-/*!
-    @brief  Sets the output voltage to a fraction of source vref.  (Value
-            can be 0..4095)
+void MCP4725::setVoltage(uint16_t output, bool writeEEPROM) {
+	bcm2835_i2c_begin();
+	bcm2835_i2c_setSlaveAddress(_i2caddr);
 
-    @param[in]  output
-                The 12-bit value representing the relationship between
-                the DAC's input voltage and its output voltage.
-    @param[in]  writeEEPROM
-                If this value is true, 'output' will also be written
-                to the MCP4725's internal non-volatile memory, meaning
-                that the DAC will retain the current voltage output
-                after power-down or reset.
-*/
-/**************************************************************************/
-void MCP4725::setVoltage( uint16_t output, bool writeEEPROM )
-{
-  
-  /*  
-    i2c_start_wait(_i2caddr);
-    
-    if (writeEEPROM)
-      i2c_write(MCP4726_CMD_WRITEDACEEPROM);
-    else
-      i2c_write(MCP4726_CMD_WRITEDAC);
-    
-    i2c_write(output / 16);                   // Upper data bits          (D11.D10.D9.D8.D7.D6.D5.D4)
-    i2c_write((output % 16) << 4);
-    
-    i2c_stop();
-    
-*/  
+	i2c_start_wait(_i2caddr);
 
+	if (writeEEPROM)
+		bcm2835_i2c_write(MCP4726_CMD_WRITEDACEEPROM);
+	else
+		bcm2835_i2c_write(MCP4726_CMD_WRITEDAC);
 
-#ifdef TWBR
-  uint8_t twbrback = TWBR;
-  TWBR = ((F_CPU / 400000L) - 16) / 2; // Set I2C frequency to 400kHz
-#endif
+	bcm2835_i2c_write(output / 16); // Upper data bits          (D11.D10.D9.D8.D7.D6.D5.D4)
+	bcm2835_i2c_write((output % 16) << 4);
 
-  Wire.beginTransmission(_i2caddr);
-
-  if (writeEEPROM)
-  {
-    Wire.write(MCP4726_CMD_WRITEDACEEPROM);
-  }
-  else
-  {
-    Wire.write(MCP4726_CMD_WRITEDAC);
-  }
-  Wire.write(output / 16);                   // Upper data bits          (D11.D10.D9.D8.D7.D6.D5.D4)
-  Wire.write((output % 16) << 4);            // Lower data bits          (D3.D2.D1.D0.x.x.x.x)
-  Wire.endTransmission();
-
-
-#ifdef TWBR
-  TWBR = twbrback;
-#endif
-
+	bcm2835_i2c_end();
 
 }
 
-void MCP4725::setVoltageFast( uint16_t output ) {
-  #ifdef TWBR
-  uint8_t twbrback = TWBR;
-  TWBR = ((F_CPU / 400000L) - 16) / 2; // Set I2C frequency to 400kHz
-  TWBR=1;
-#endif
-
- Wire.beginTransmission(_i2caddr);
-  Wire.write(output>>8);                   // Upper data bits          (D11.D10.D9.D8.D7.D6.D5.D4)
-  Wire.write(output);            // Lower data bits          (D3.D2.D1.D0.x.x.x.x)
-  Wire.endTransmission();
-
-#ifdef TWBR
-  TWBR = twbrback;
-#endif
+void MCP4725::setVoltageFast(uint16_t output) {
+	bcm2835_i2c_begin();
+	bcm2835_i2c_setSlaveAddress(_i2caddr);
+	bcm2835_i2c_write(volt >> 8);
+	bcm2835_i2c_write (volt);
+	bcm2835_i2c_end();
 
 }
 
diff --git a/src/dac.h b/src/dac.h
index 8b7ecbb..ab40d8b 100644
--- a/src/dac.h
+++ b/src/dac.h
@@ -1,6 +1,7 @@
 #pragma once
 
-#include <inttypes.h>
+#include <sys/types.h>
+#include <bcm2835.h>
 
 class Dac {
 public:
diff --git a/src/main.c b/src/main.c
index c842df0..6b1e54a 100644
--- a/src/main.c
+++ b/src/main.c
@@ -1,5 +1,4 @@
-#include <bcm2835.h>
-#include <sys/types.h>
+
 
 int dacX = 0x60;
 int dacY = 0x61;
@@ -27,17 +26,6 @@
     return 0;
 }
 
-int i2c_init() {
-
-	if (!bcm2835_init())
-		return(1);
-
-	bcm2835_i2c_set_baudrate(10000);
-
-	return(0);
-}
-
-
 mcp4725_setVoltageFast(int dac, u_int16_t volt) {
   bcm2835_i2c_begin();
   bcm2835_i2c_setSlaveAddress(dac);