diff --git a/Makefile b/Makefile
new file mode 100644
index 0000000..c223352
--- /dev/null
+++ b/Makefile
@@ -0,0 +1,110 @@
+TARGET=ubScopin
+
+CC = gcc
+CXX = g++
+
+OBJCOPY=avr-objcopy
+
+HEX_FLASH_FLAGS=--pad-to 0x2000
+
+OPTIM=-O2
+WARNINGS=-Wall
+
+BUILD=build
+SOURCES=src
+
+INCLUDES=src
+
+COMMON = 
+CFLAGS  = $(COMMON) -std=c99
+CFLAGS += $(WARNINGS) $(OPTIM) $(WARNINGS) $(INCLUDE)
+CFLAGS += -MD -MP -MT $(*F).o -MF $(DEPSDIR)/$(@F).d
+
+CXXFLAGS = $(COMMON)
+CXXFLAGS+= $(WARNINGS) $(OPTIM) $(WARNINGS) $(INCLUDE)
+CXXFLAGS+= -MD -MP -MT $(*F).o -MF $(DEPSDIR)/$(@F).d
+CXXFLAGS+= -ffunction-sections -Wl,--gc-sections
+
+## Assembly specific flags
+ASMFLAGS = $(COMMON)
+ASMFLAGS += $(CFLAGS)
+ASMFLAGS += -x assembler-with-cpp
+
+## Linker flags
+LDFLAGS = $(COMMON)
+LDFLAGS +=
+
+ifneq ($(BUILD),$(notdir $(CURDIR)))
+
+export MAKEFILE = $(CURDIR)/Makefile
+
+export INCLUDE = $(foreach dir,$(INCLUDES),-I$(CURDIR)/$(dir)) \
+        $(foreach dir,$(LIBDIRS),-I$(dir)/include) \
+        -I$(CURDIR)/$(BUILD)
+
+export OUTPUT   :=  $(CURDIR)/$(TARGET)
+export VPATH    :=  $(foreach dir,$(SOURCES),$(CURDIR)/$(dir)) \
+            $(foreach dir,$(DATA),$(CURDIR)/$(dir))
+export DEPSDIR  :=  $(CURDIR)/$(BUILD)
+
+export CFILES   = $(foreach dir,$(SOURCES),$(notdir $(wildcard $(dir)/*.c)))
+export SFILES   = $(foreach dir,$(SOURCES),$(notdir $(wildcard $(dir)/*.S)))
+export CPPFILES = $(foreach dir,$(SOURCES),$(notdir $(wildcard $(dir)/*.cpp)))
+export AUTOSRC = $(AUTOC) $(AUTOCPP)
+
+#
+# Define all object files.
+#
+
+export OFILES   := $(CPPFILES:.cpp=.o) $(CFILES:.c=.o) $(SFILES:.S=.o)
+
+.PHONY: $(BUILD) clean 
+
+# ---------------------- 
+
+$(BUILD):
+	@[ -d $@ ] || mkdir -p $@
+	@#@make --no-print-directory -C $(BUILD) -f $(CURDIR)/Makefile
+	@make -C $(BUILD) -f $(MAKEFILE)
+
+all:    $(BUILD) size
+
+clean:
+	rm -rf $(BUILD) $(TARGET).elf $(TARGET).{hex,map,elf,lss} html
+
+else
+
+DEPENDS :=  $(OFILES:.o=.d)
+
+bazola:	$(OUTPUT).hex $(OUTPUT).lss size
+
+$(OUTPUT).lss:	$(OUTPUT).elf
+
+$(OUTPUT).hex:  $(OUTPUT).elf
+
+$(OUTPUT).elf:  $(OFILES)
+
+size:	$(OUTPUT).elf
+	@echo
+	@avr-size $(OUTPUT).elf
+
+-include $(DEPENDS)
+
+endif
+
+$(OUTPUT).hex:	$(OUTPUT).elf
+	$(OBJCOPY) $(OUTPUT).elf -O ihex $(HEX_FLASH_FLAGS) $(OUTPUT).hex
+
+$(OUTPUT).lss:	$(OUTPUT).elf
+	avr-objdump -h -S $< > $@
+
+$(OUTPUT).elf : $(OFILES) $(MAKEFILE)
+	$(CXX) $(CXXFLAGS) $(OFILES) $(LINKER_FLAGS) -o$(OUTPUT).elf
+
+%.o:    %.c
+	@echo $(notdir $<)
+	$(CC) $(CFLAGS) -c $< -o $@ $(ERROR_FILTER)
+
+%.o:	%.S
+	@echo $(notdir $<)
+	$(CC) $(ASMFLAGS) -c $< -o $@ $(ERROR_FILTER)
diff --git a/main.c b/main.c
deleted file mode 100644
index 35f1094..0000000
--- a/main.c
+++ /dev/null
@@ -1,47 +0,0 @@
-#include <bcm2835.h>
-#include <sys/types.h>
-
-int dacX = 0x60;
-int dacY = 0x61;
-
-int main(int argc, char **argv)
-{
-    char buf[1];
-
-    if (!bcm2835_init())return 1;
-    bcm2835_i2c_begin();                //Start I2C operations.
-    bcm2835_i2c_setSlaveAddress(0x20);  //I2C address
-    bcm2835_i2c_set_baudrate(10000);    //1M baudrate
-
-    while(1)
-    {
-        buf[0] = 0xEF;    //LED ON
-        bcm2835_i2c_write(buf,1);
-        bcm2835_delay(500);
-        buf[0] = 0xFF;     //LED OFF
-        bcm2835_i2c_write(buf,1);
-        bcm2835_delay(500);
-    }
-    bcm2835_i2c_end();
-    bcm2835_close();
-    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);
-  bcm2835_i2c_write(volt>>8);
-  bcm2835_i2c_write(volt);
-  bcm2835_i2c_end();
-}
diff --git a/src/charset0.c b/src/charset0.c
new file mode 100644
index 0000000..4d1ea5d
--- /dev/null
+++ b/src/charset0.c
@@ -0,0 +1,424 @@
+/*
+   Copyright (c) 1992 - 1994 Heinz W. Werntges.  All rights reserved.
+   Parts Copyright (c) 1999  Martin Kroeker  All rights reserved.
+
+   Distributed by Free Software Foundation, Inc.
+
+  This file is part of HP2xx.
+
+  HP2xx is distributed in the hope that it will be useful, but
+  WITHOUT ANY WARRANTY.  No author or distributor accepts responsibility
+  to anyone for the consequences of using it or for whether it serves any
+  particular purpose or works at all, unless he says so in writing.  Refer
+  to the GNU General Public License, Version 2 or later, for full details.
+
+  Everyone is granted permission to copy, modify and redistribute
+  HP2xx, but only under the conditions described in the GNU General Public
+  License.  A copy of this license is supposed to have been
+  given to you along with HP2xx so you can know your rights and
+  responsibilities.  It should be in a file named COPYING.  Among other
+  things, the copyright notice and this notice must be preserved on all
+  copies.
+
+  - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
+*/
+
+/**
+ ** This file defines a standard character set by elementary
+ ** "draw" & "move" commands. The format is a very compact one from
+ ** the old days where every byte was still appreciated.
+ **
+ ** A font or character set is an array of strings. Each character
+ ** corresponds to one of these strings, which is addressed by its ASCII code.
+ **
+ ** A character is a (NULL-terminated) string of bytes. Each byte
+ ** codes for a draw or move action according to the code below:
+ **
+ **  Bit:  7 6 5 4 3 2 1 0
+ **   p x x x y y y y
+ **
+ ** p:  Plot flag. If set, "draw to" new point, else "move to" it.
+ ** xxx:  3-bit unsigned integer  (0...7). X coordinate of new point.
+ ** yyyy: 4-bit unsigned integer (0..15). Y coordinate of new point.
+ **
+ ** The baseline is y = 4 instead of y = 0, so characters with parts
+ ** below it can be drawn properly without a need for sign bits.
+ ** Function "code_to_ucoord()" transforms these coordinates into
+ ** actual user coordinates.
+ **
+ ** Example:  code for character 'L': "\032\224\324" translates to:
+ **   moveto(1,10); drawto(1,4); drawto(5,4);
+ **
+ ** From the example you can conclude that the font below essentially is
+ ** defined on a 5x7 grid:
+ **
+ **     0 1 2 3 4 5 6 7
+ ** 15  . . . . . . . .   . : unused
+ ** 14  . . . . . . . .   * : always used
+ ** 13  . . . . . . . .   o : sometimes used
+ ** 12  . . . . . . . .
+ ** 11  . . . . . . . .
+ ** 10  o * * * * * . .
+ **  9  o * * * * * . .
+ **  8  o * * * * * . .
+ **  7  o * * * * * . .
+ **  6  o * * * * * . .
+ **  5  o * * * * * . .
+ **  4  o * * * * * . .
+ **  3  o o o o o o . .
+ **  2  o o o o o o . .
+ **  1  o o o o o o . .
+ **  0  o o o o o o . .
+ **/
+
+
+/**
+ ** The following array of strings contains the basic character set (set 0).
+ **
+ ** NOTE: A nice way to add a new charset would be, e. g., to introduce a
+ ** ``charset1[]'' as the "alternate" charset and implement the HP-GL
+ ** commands needed for switching from one to the other.
+ **/
+
+#include <avr/pgmspace.h>
+
+/* 0x00 ... 0x1f        */
+
+/**
+ ** Unfortunately, some compilers do not process \xNN properly,
+ ** so I changed all hex codes (\xNN) into octal codes (\NNN),
+ ** thereby losing readability but gaining portability.
+ **/
+
+/* 0x20 ... 0x2f        */
+const char chr20[] PROGMEM =  "";
+const char chr21[] PROGMEM =  "\064\265\066\272";
+const char chr22[] PROGMEM =  "\051\252\111\312";
+const char chr23[] PROGMEM =  "\044\252\104\312\026\326\030\330";
+const char chr24[] PROGMEM =  "\064\272\131\251\230\247\307\326\305\225";
+const char chr25[] PROGMEM =  "\024\332\051\250\270\271\251\066\265\305\306\266";
+const char chr26[] PROGMEM =  "\124\230\231\252\271\270\226\225\244\264\326";
+const char chr27[] PROGMEM =  "\071\312";
+const char chr28[] PROGMEM =  "\132\270\266\324";
+const char chr29[] PROGMEM =  "\024\266\270\232";
+const char chr2a[] PROGMEM =  "\005\351\145\211\072\264";
+const char chr2b[] PROGMEM =  "\065\271\027\327";
+const char chr2c[] PROGMEM =  "\064\244\245\265\263\242";
+const char chr2d[] PROGMEM =  "\027\327";
+const char chr2e[] PROGMEM =  "\064\244\245\265\264";
+const char chr2f[] PROGMEM =  "\352";
+
+/* 0x30 ... 0x3f        */
+/*
+  "\025\244\304\325\331\312\252\231\225\331", ** Zero including `/' **
+*/
+const char chr30[] PROGMEM =  "\025\244\304\325\331\312\252\231\225";
+const char chr31[] PROGMEM =  "\044\304\064\272\251";
+const char chr32[] PROGMEM =  "\031\252\312\331\330\225\224\324";
+const char chr33[] PROGMEM =  "\025\244\304\325\326\307\267\332\232";
+const char chr34[] PROGMEM =  "\112\227\226\326\107\304";
+const char chr35[] PROGMEM =  "\132\232\230\310\327\325\304\244\225";
+const char chr36[] PROGMEM =  "\132\272\230\225\244\304\325\326\307\227";
+const char chr37[] PROGMEM =  "\032\332\331\226\224";
+const char chr38[] PROGMEM =  "\107\330\331\312\252\231\230\247\307\326\325\304\244\225\226\247";
+const char chr39[] PROGMEM =  "\044\264\326\331\312\252\231\230\247\327";
+const char chr3a[] PROGMEM =  "\047\250\270\267\247\045\265\264\244\245";
+const char chr3b[] PROGMEM =  "\046\247\267\266\246\064\244\245\265\263\242";
+const char chr3c[] PROGMEM =  "\112\227\304";
+const char chr3d[] PROGMEM =  "\030\330\026\326";
+const char chr3e[] PROGMEM =  "\032\307\224";
+const char chr3f[] PROGMEM =  "\031\252\312\331\330\307\267\266\065\264";
+
+/* 0x40 ... 0x4f        */
+const char chr40[] PROGMEM =  "\103\243\224\230\252\312\331\326\305\266\267\310\330";
+const char chr41[] PROGMEM =  "\024\231\252\312\331\324\026\326";
+const char chr42[] PROGMEM =  "\024\232\312\331\330\307\227\024\304\325\326\307";
+const char chr43[] PROGMEM =  "\125\304\244\225\231\252\312\331";
+const char chr44[] PROGMEM =  "\024\232\312\331\325\304\224";
+const char chr45[] PROGMEM =  "\124\224\232\332\027\307";
+const char chr46[] PROGMEM =  "\024\232\332\027\307";
+const char chr47[] PROGMEM =  "\131\312\252\231\225\244\304\325\327\247";
+const char chr48[] PROGMEM =  "\024\232\124\332\027\327";
+const char chr49[] PROGMEM =  "\024\324\064\272\032\332";
+const char chr4a[] PROGMEM =  "\025\244\304\325\332\232";
+const char chr4b[] PROGMEM =  "\024\232\027\247\324\047\332";
+const char chr4c[] PROGMEM =  "\032\224\324";
+const char chr4d[] PROGMEM =  "\024\232\270\332\324";
+const char chr4e[] PROGMEM =  "\024\232\324\332";
+const char chr4f[] PROGMEM =  "\044\225\231\252\312\331\325\304\244";
+
+/* 0x50 ... 0x5f        */
+const char chr50[] PROGMEM =  "\024\232\312\331\330\307\227";
+const char chr51[] PROGMEM =  "\044\225\231\252\312\331\326\264\244\066\324";
+const char chr52[] PROGMEM =  "\024\232\312\331\330\307\227\247\324";
+const char chr53[] PROGMEM =  "\025\244\304\325\326\307\247\230\231\252\312\331";
+const char chr54[] PROGMEM =  "\064\272\232\332";
+const char chr55[] PROGMEM =  "\032\225\244\304\325\332";
+const char chr56[] PROGMEM =  "\032\230\264\330\332";
+const char chr57[] PROGMEM =  "\032\224\267\324\332";
+const char chr58[] PROGMEM =  "\024\332\124\232";
+const char chr59[] PROGMEM =  "\032\231\266\264\066\331\332";
+const char chr5a[] PROGMEM =  "\032\332\224\324";
+const char chr5b[] PROGMEM =  "\124\264\272\332";
+const char chr5c[] PROGMEM =  "\032\324";
+const char chr5d[] PROGMEM =  "\024\264\272\232";
+const char chr5e[] PROGMEM =  "\030\272\330";
+const char chr5f[] PROGMEM =  "\023\323";
+
+/* 0x60 ... 0x6f        */
+const char chr60[] PROGMEM =  "\053\310";
+const char chr61[] PROGMEM =  "\124\244\225\227\250\310\304";
+const char chr62[] PROGMEM =  "\024\304\325\327\310\250\052\244";
+const char chr63[] PROGMEM =  "\125\304\264\245\247\270\310\327";
+const char chr64[] PROGMEM =  "\112\304\244\225\227\250\310\104\324";
+const char chr65[] PROGMEM =  "\026\306\327\310\250\227\225\244\324";
+const char chr66[] PROGMEM =  "\064\271\312\332\047\307";
+const char chr67[] PROGMEM =  "\022\262\303\310\250\227\225\244\304";
+const char chr68[] PROGMEM =  "\032\224\030\270\307\304";
+const char chr69[] PROGMEM =  "\072\271\050\270\264\044\304";
+const char chr6a[] PROGMEM =  "\072\271\050\270\263\242\222";
+const char chr6b[] PROGMEM =  "\024\232\104\226\310";
+const char chr6c[] PROGMEM =  "\052\272\264\044\304";
+const char chr6d[] PROGMEM =  "\024\230\027\250\267\264\067\310\327\324";
+const char chr6e[] PROGMEM =  "\024\230\027\250\270\307\304";
+const char chr6f[] PROGMEM =  "\044\225\227\250\270\307\305\264\244";
+
+/* 0x70 ... 0x7f        */
+const char chr70[] PROGMEM =  "\022\230\270\307\305\264\224";
+const char chr71[] PROGMEM =  "\104\244\225\227\250\310\302";
+const char chr72[] PROGMEM =  "\030\224\026\270\310";
+const char chr73[] PROGMEM =  "\110\250\227\246\266\305\264\224";
+const char chr74[] PROGMEM =  "\052\244\304\030\310";
+const char chr75[] PROGMEM =  "\030\225\244\304\310";
+const char chr76[] PROGMEM =  "\030\226\264\326\330";
+const char chr77[] PROGMEM =  "\030\225\244\265\267\065\304\325\330";
+const char chr78[] PROGMEM =  "\030\324\024\330";
+const char chr79[] PROGMEM =  "\022\326\330\030\226\264";
+const char chr7a[] PROGMEM =  "\030\310\224\304";
+const char chr7b[] PROGMEM =  "\113\273\252\250\227\246\244\263\303";
+const char chr7c[] PROGMEM =  "\073\263";
+const char chr7d[] PROGMEM =  "\053\273\312\310\327\306\304\263\243";
+const char chr7e[] PROGMEM =  "\031\252\310\331";
+const char chr7f[] PROGMEM =  "";
+
+PGM_P const charset0[256] PROGMEM = {
+  chr20,
+  chr21,
+  chr22,
+  chr23,
+  chr24,
+  chr25,
+  chr26,
+  chr27,
+  chr28,
+  chr29,
+  chr2a,
+  chr2b,
+  chr2c,
+  chr2d,
+  chr2e,
+  chr2f,
+
+  /* 0x30 ... 0x3f        */
+  /*
+    "\025\244\304\325\331\312\252\231\225\331", ** Zero including `/' **
+  */
+  chr30,
+  chr31,
+  chr32,
+  chr33,
+  chr34,
+  chr35,
+  chr36,
+  chr37,
+  chr38,
+  chr39,
+  chr3a,
+  chr3b,
+  chr3c,
+  chr3d,
+  chr3e,
+  chr3f,
+
+  /* 0x40 ... 0x4f        */
+  chr40,
+  chr41,
+  chr42,
+  chr43,
+  chr44,
+  chr45,
+  chr46,
+  chr47,
+  chr48,
+  chr49,
+  chr4a,
+  chr4b,
+  chr4c,
+  chr4d,
+  chr4e,
+  chr4f,
+
+  /* 0x50 ... 0x5f        */
+  chr50,
+  chr51,
+  chr52,
+  chr53,
+  chr54,
+  chr55,
+  chr56,
+  chr57,
+  chr58,
+  chr59,
+  chr5a,
+  chr5b,
+  chr5c,
+  chr5d,
+  chr5e,
+  chr5f,
+
+  /* 0x60 ... 0x6f        */
+  chr60,
+  chr61,
+  chr62,
+  chr63,
+  chr64,
+  chr65,
+  chr66,
+  chr67,
+  chr68,
+  chr69,
+  chr6a,
+  chr6b,
+  chr6c,
+  chr6d,
+  chr6e,
+  chr6f,
+
+  /* 0x70 ... 0x7f        */
+  chr70,
+  chr71,
+  chr72,
+  chr73,
+  chr74,
+  chr75,
+  chr76,
+  chr77,
+  chr78,
+  chr79,
+  chr7a,
+  chr7b,
+  chr7c,
+  chr7d,
+  chr7e,
+  chr7f,
+
+  chr20,
+  chr21,
+  chr22,
+  chr23,
+  chr24,
+  chr25,
+  chr26,
+  chr27,
+  chr28,
+  chr29,
+  chr2a,
+  chr2b,
+  chr2c,
+  chr2d,
+  chr2e,
+  chr2f,
+
+  /* 0x30 ... 0x3f        */
+  /*
+    "\025\244\304\325\331\312\252\231\225\331", ** Zero including `/' **
+  */
+  chr30,
+  chr31,
+  chr32,
+  chr33,
+  chr34,
+  chr35,
+  chr36,
+  chr37,
+  chr38,
+  chr39,
+  chr3a,
+  chr3b,
+  chr3c,
+  chr3d,
+  chr3e,
+  chr3f,
+
+  /* 0x40 ... 0x4f        */
+  chr40,
+  chr41,
+  chr42,
+  chr43,
+  chr44,
+  chr45,
+  chr46,
+  chr47,
+  chr48,
+  chr49,
+  chr4a,
+  chr4b,
+  chr4c,
+  chr4d,
+  chr4e,
+  chr4f,
+
+  /* 0x50 ... 0x5f        */
+  chr50,
+  chr51,
+  chr52,
+  chr53,
+  chr54,
+  chr55,
+  chr56,
+  chr57,
+  chr58,
+  chr59,
+  chr5a,
+  chr5b,
+  chr5c,
+  chr5d,
+  chr5e,
+  chr5f,
+
+  /* 0x60 ... 0x6f        */
+  chr60,
+  chr61,
+  chr62,
+  chr63,
+  chr64,
+  chr65,
+  chr66,
+  chr67,
+  chr68,
+  chr69,
+  chr6a,
+  chr6b,
+  chr6c,
+  chr6d,
+  chr6e,
+  chr6f,
+
+  /* 0x70 ... 0x7f        */
+  chr70,
+  chr71,
+  chr72,
+  chr73,
+  chr74,
+  chr75,
+  chr76,
+  chr77,
+  chr78,
+  chr79,
+  chr7a,
+  chr7b,
+  chr7c,
+  chr7d,
+  chr7e,
+  chr7f,
+};
diff --git a/src/config.h b/src/config.h
new file mode 100644
index 0000000..5ce822a
--- /dev/null
+++ b/src/config.h
@@ -0,0 +1,38 @@
+#pragma once
+
+#include <avr/io.h>
+
+#define TP(x,y) x ## y 
+#define TP2(x,y) TP(x,y)
+
+#define ZDACPORT    C
+#define PORT_ZDAC   TP2(PORT,ZDACPORT)
+#define DDR_ZDAC    TP2(DDR,ZDACPORT)
+#define ZDAC        1
+#define ZDAC_BV     _BV(ZDAC)
+
+#define RTCSSPORT   C
+#define PORT_RTCSS  TP2(PORT,RTCSSPORT)
+#define DDR_RTCSS   TP2(DDR,RTCSSPORT)
+#define RTCSS       2
+#define RTCSEL      RTCSS
+#define RTCSS_BV    _BV(RTCSS)
+
+
+#define MOSIPORT    B
+#define PORT_MOSI   TP2(PORT,MOSIPORT)
+#define DDR_MOSI    TP2(DDR,MOSIPORT)
+//#define MOSI        PB3
+#define MOSIBV      _BV(MOSI)
+
+#define MISOPORT    B
+#define PORT_MISO   TP2(PORT,MISOPORT)
+#define DDR_MISO    TP2(DDR,MISOPORT)
+//#define MISO        PB4
+#define MISOBV      _BV(MISO)
+
+#define SCKPORT     B
+#define PORT_SCK    TP2(PORT,SCKPORT)
+#define DDR_SCK     TP2(DDR,SCKPORT)
+//#define SCK         PB5
+#define SCKBV       _BV(SCK)
diff --git a/src/dac.cpp b/src/dac.cpp
new file mode 100644
index 0000000..25fb628
--- /dev/null
+++ b/src/dac.cpp
@@ -0,0 +1,127 @@
+#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();
+
+}
+
+void Dac::SetXY(int dacX, int dacY) {
+  //xDac.setVoltageFast(dacX * 3, false);
+  //yDac.setVoltageFast(dacY * 3, false);
+
+  //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);
+}
+
+MCP4725::MCP4725() {
+}
+
+
+void MCP4725::begin(uint8_t addr) {
+  _i2caddr = addr;
+}
+
+/**************************************************************************/
+/*!
+    @brief  Sets the output voltage to a fraction of source vref.  (Value
+            can be 0..4095)
+
+    @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();
+    
+*/  
+
+
+#ifdef TWBR
+  uint8_t twbrback = TWBR;
+  TWBR = ((F_CPU / 400000L) - 16) / 2; // Set I2C frequency to 400kHz
+#endif
+
+  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
+
+
+}
+
+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
+
+}
+
diff --git a/src/dac.h b/src/dac.h
new file mode 100644
index 0000000..8b7ecbb
--- /dev/null
+++ b/src/dac.h
@@ -0,0 +1,28 @@
+#pragma once
+
+#include <inttypes.h>
+
+class Dac {
+public:
+    void Setup();
+    void SetXY(int dacX, int dacY);
+    void SetZ(uint8_t dacZ);
+};
+
+#define MCP4726_CMD_WRITEDAC            (0x40)  // Writes data to the DAC
+#define MCP4726_CMD_WRITEDACEEPROM      (0x60)  // Writes data to the DAC and the EEPROM (persisting the assigned value after reset)
+
+class MCP4725{
+ public:
+  MCP4725();
+  void begin(uint8_t a);  
+  void setVoltage( uint16_t output, bool writeEEPROM );
+  void setVoltageFast( uint16_t output);
+
+ private:
+  uint8_t _i2caddr;
+};
+
+extern Dac xyz;
+extern MCP4725 xDac;
+extern MCP4725 yDac;
diff --git a/src/i2cmaster.h b/src/i2cmaster.h
new file mode 100644
index 0000000..b97a55e
--- /dev/null
+++ b/src/i2cmaster.h
@@ -0,0 +1,183 @@
+#ifndef _I2CMASTER_H
+#define _I2CMASTER_H
+/************************************************************************* 
+* Title:    C include file for the I2C master interface 
+*           (i2cmaster.S or twimaster.c)
+* Author:   Peter Fleury <pfleury@gmx.ch>
+* File:     $Id: i2cmaster.h,v 1.12 2015/09/16 09:27:58 peter Exp $
+* Software: AVR-GCC 4.x
+* Target:   any AVR device
+* Usage:    see Doxygen manual
+**************************************************************************/
+
+/**
+ @file
+ @defgroup pfleury_ic2master I2C Master library
+ @code #include <i2cmaster.h> @endcode
+  
+ @brief I2C (TWI) Master Software Library
+
+ Basic routines for communicating with I2C slave devices. This single master 
+ implementation is limited to one bus master on the I2C bus. 
+
+ This I2c library is implemented as a compact assembler software implementation of the I2C protocol 
+ which runs on any AVR (i2cmaster.S) and as a TWI hardware interface for all AVR with built-in TWI hardware (twimaster.c).
+ Since the API for these two implementations is exactly the same, an application can be linked either against the
+ software I2C implementation or the hardware I2C implementation.
+
+ Use 4.7k pull-up resistor on the SDA and SCL pin.
+ 
+ Adapt the SCL and SDA port and pin definitions and eventually the delay routine in the module 
+ i2cmaster.S to your target when using the software I2C implementation ! 
+ 
+ Adjust the  CPU clock frequence F_CPU in twimaster.c or in the Makfile when using the TWI hardware implementaion.
+
+ @note 
+    The module i2cmaster.S is based on the Atmel Application Note AVR300, corrected and adapted 
+    to GNU assembler and AVR-GCC C call interface.
+    Replaced the incorrect quarter period delays found in AVR300 with 
+    half period delays. 
+    
+ @author Peter Fleury pfleury@gmx.ch  http://tinyurl.com/peterfleury
+ @copyright (C) 2015 Peter Fleury, GNU General Public License Version 3
+ 
+ @par API Usage Example
+  The following code shows typical usage of this library, see example test_i2cmaster.c
+
+ @code
+
+ #include <i2cmaster.h>
+
+
+ #define Dev24C02  0xA2      // device address of EEPROM 24C02, see datasheet
+
+ int main(void)
+ {
+     unsigned char ret;
+
+     i2c_init();                             // initialize I2C library
+
+     // write 0x75 to EEPROM address 5 (Byte Write) 
+     i2c_start_wait(Dev24C02+I2C_WRITE);     // set device address and write mode
+     i2c_write(0x05);                        // write address = 5
+     i2c_write(0x75);                        // write value 0x75 to EEPROM
+     i2c_stop();                             // set stop conditon = release bus
+
+
+     // read previously written value back from EEPROM address 5 
+     i2c_start_wait(Dev24C02+I2C_WRITE);     // set device address and write mode
+
+     i2c_write(0x05);                        // write address = 5
+     i2c_rep_start(Dev24C02+I2C_READ);       // set device address and read mode
+
+     ret = i2c_readNak();                    // read one byte from EEPROM
+     i2c_stop();
+
+     for(;;);
+ }
+ @endcode
+
+*/
+
+
+/**@{*/
+
+#if (__GNUC__ * 100 + __GNUC_MINOR__) < 304
+#error "This library requires AVR-GCC 3.4 or later, update to newer AVR-GCC compiler !"
+#endif
+
+#include <avr/io.h>
+
+/** defines the data direction (reading from I2C device) in i2c_start(),i2c_rep_start() */
+#define I2C_READ    1
+
+/** defines the data direction (writing to I2C device) in i2c_start(),i2c_rep_start() */
+#define I2C_WRITE   0
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ @brief initialize the I2C master interace. Need to be called only once 
+ @return none
+ */
+extern void i2c_init(void);
+
+
+/** 
+ @brief Terminates the data transfer and releases the I2C bus 
+ @return none
+ */
+extern void i2c_stop(void);
+
+
+/** 
+ @brief Issues a start condition and sends address and transfer direction 
+  
+ @param    addr address and transfer direction of I2C device
+ @retval   0   device accessible 
+ @retval   1   failed to access device 
+ */
+extern unsigned char i2c_start(unsigned char addr);
+
+
+/*
+ @brief Issues a repeated start condition and sends address and transfer direction 
+
+ @param   addr address and transfer direction of I2C device
+ @retval  0 device accessible
+ @retval  1 failed to access device
+ */
+extern unsigned char i2c_rep_start(unsigned char addr);
+
+
+/**
+ @brief Issues a start condition and sends address and transfer direction 
+   
+ If device is busy, use ack polling to wait until device ready 
+ @param    addr address and transfer direction of I2C device
+ @return   none
+ */
+extern void i2c_start_wait(unsigned char addr);
+
+ 
+/**
+ @brief Send one byte to I2C device
+ @param    data  byte to be transfered
+ @retval   0 write successful
+ @retval   1 write failed
+ */
+extern unsigned char i2c_write(unsigned char data);
+
+
+/**
+ @brief    read one byte from the I2C device, request more data from device 
+ @return   byte read from I2C device
+ */
+extern unsigned char i2c_readAck(void);
+
+/**
+ @brief    read one byte from the I2C device, read is followed by a stop condition 
+ @return   byte read from I2C device
+ */
+extern unsigned char i2c_readNak(void);
+
+/** 
+ @brief    read one byte from the I2C device
+ 
+ Implemented as a macro, which calls either @ref i2c_readAck or @ref i2c_readNak
+ 
+ @param    ack 1 send ack, request more data from device<br>
+               0 send nak, read is followed by a stop condition 
+ @return   byte read from I2C device
+ */
+extern unsigned char i2c_read(unsigned char ack);
+#define i2c_read(ack)  (ack) ? i2c_readAck() : i2c_readNak(); 
+
+#ifdef __cplusplus
+}
+#endif
+
+/**@}*/
+#endif
diff --git a/src/line.cpp b/src/line.cpp
new file mode 100644
index 0000000..37250a9
--- /dev/null
+++ b/src/line.cpp
@@ -0,0 +1,94 @@
+#include <inttypes.h>
+#include <stdlib.h>
+
+#include "dac.h"
+#include "line.h"
+
+Tracer trazador;
+
+#define SWAP(a,b) {(a)^=(b);(b)^=(a);(a)^=(b);}
+
+int Tracer::clamp(int *x) const
+{
+  if (*x < 0) {
+    *x = 0;
+  }
+  else if (*x > 255) {
+    *x = 255;
+  }
+
+  return 0;
+}
+
+int Tracer::MoveTo(int x, int y)
+{
+  X = x;
+  Y = y;
+  clamp(&x);
+  clamp(&y);
+  xyz.SetXY(x, y);
+
+  return 0;
+}
+
+void Tracer::LineTo(int x1, int y1)
+{
+  int x0 = X;
+  int y0 = Y;
+  int Dx = x1 - x0;
+  int Dy = y1 - y0;
+  int steep = abs(Dy) >= abs(Dx);
+  int xstep, ystep;
+  int twoDy, twoDyTwoDx, E, x, y, xend;
+
+
+  if ((x1 > 255 || x1 < 0 || y1 > 255 || y1 < 0) &&
+      (X  > 255 || X  < 0 || Y  > 255 || Y  < 0))  {
+    X = x1;
+    Y = y1;
+    return;
+  }
+
+  if (steep) {
+    SWAP(x0, y0);
+    SWAP(x1, y1);
+    Dx = x1 - x0;
+    Dy = y1 - y0;
+  }
+
+  xstep = 1;
+  if (Dx < 0) {
+    xstep = -1;
+    Dx = -Dx;
+  }
+
+  ystep = 1;
+  if (Dy < 0) {
+    ystep = -1;
+    Dy = -Dy;
+  }
+
+  twoDy = Dy << 1;
+  twoDyTwoDx = twoDy - (Dx << 1);
+  E = twoDy - Dx;
+  y = y0;
+  x = x0;
+  xend = x1;
+
+  for (; x != xend;) {
+    if (E > 0) {
+      E += twoDyTwoDx;
+      y += ystep;
+    } else {
+      E += twoDy;
+    }
+    x += xstep;
+
+    if (steep) {
+      MoveTo(y, x);
+    } else {
+      MoveTo(x, y);
+    }
+    makePace();
+  }
+}
diff --git a/src/line.h b/src/line.h
new file mode 100644
index 0000000..1812386
--- /dev/null
+++ b/src/line.h
@@ -0,0 +1,20 @@
+#pragma once
+
+#include <inttypes.h>
+
+class Tracer {
+private:
+    uint8_t pace;
+    int clamp(int* x) const;
+    inline void makePace() const { for(uint8_t j = 0; j < pace; j++)
+                                   __asm volatile("nop"); };
+
+public:
+    int X, Y;
+
+    inline void SetPace(uint8_t pace) { this->pace = pace; }
+    int MoveTo(int x, int y);
+    void LineTo(int x1, int y1);
+};
+
+extern Tracer trazador;
diff --git a/src/main.c b/src/main.c
new file mode 100644
index 0000000..c842df0
--- /dev/null
+++ b/src/main.c
@@ -0,0 +1,207 @@
+#include <bcm2835.h>
+#include <sys/types.h>
+
+int dacX = 0x60;
+int dacY = 0x61;
+
+int main(int argc, char **argv)
+{
+    char buf[1];
+
+    if (!bcm2835_init())return 1;
+    bcm2835_i2c_begin();                //Start I2C operations.
+    bcm2835_i2c_setSlaveAddress(0x20);  //I2C address
+    bcm2835_i2c_set_baudrate(10000);    //1M baudrate
+
+    while(1)
+    {
+        buf[0] = 0xEF;    //LED ON
+        bcm2835_i2c_write(buf,1);
+        bcm2835_delay(500);
+        buf[0] = 0xFF;     //LED OFF
+        bcm2835_i2c_write(buf,1);
+        bcm2835_delay(500);
+    }
+    bcm2835_i2c_end();
+    bcm2835_close();
+    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);
+  bcm2835_i2c_write(volt>>8);
+  bcm2835_i2c_write(volt);
+  bcm2835_i2c_end();
+}
+#include "util.h"
+#include "config.h"
+#include "dac.h"
+#include "pecado.h"
+#include "text.h"
+#include "line.h"
+#include "shape.h"
+
+void setup() {
+  Serial.begin(9600);
+  xyz.Setup();
+    //digitalWrite(A4, LOW);
+//digitalWrite(A5, LOW);
+}
+
+void loop() {
+
+  //draw_spiral();
+  //draw_marchingborder();
+  draw_xyxor();
+}
+
+int16_t scale = 0;
+int16_t scale_sin = 1;
+int8_t  scale_sin_d = -1;
+int16_t textscale = 4 * 256;
+int16_t  d_textscale = 100;
+uint8_t a;
+uint16_t textangle = 0;
+
+uint8_t centre_x, centre_y;
+uint16_t centreangle = 0;
+
+#define XYXOR_WORD "ZLOSHA"
+#define XYXOR_LEN  3
+
+void draw_marchingborder()
+{
+
+  trazador.SetPace(0);
+  for (int i = -1, f = 0; i < 256; i += 8, f++) {
+    int16_t x = 128 + isin(i);
+    int16_t y = 128 + icos(i);
+    if (i == -1 || ((f + textangle / 4) % 2 == 0)) {
+      trazador.MoveTo(x, y);
+    }
+    else {
+      trazador.LineTo(x, y);
+    }
+  }
+}
+
+void draw_spiral()
+{
+  for (int i = -1; i < 256; i += 8) {
+    int16_t x = isin(a);
+    int16_t y = icos(a);
+    x = 128 + (x * scale) / 128;
+    y = 128 + (y * scale) / 128;
+    if (i == -1) {
+      trazador.MoveTo(x, y);
+    }
+    else {
+      trazador.LineTo(x, y);
+    }
+    scale += 4;
+    a += 8;
+  }
+}
+
+char timetext[6];
+char bigtext[17];
+uint16_t time;
+uint16_t frameno;
+
+void updateText()
+{
+  timetext[0] = '0' + (time >> 12);
+  timetext[1] = '0' + ((time >> 8) & 15);
+  timetext[2] = (frameno & 64) == 0 ? ':' : ' ';
+  timetext[3] = '0' + ((time >> 4) & 15);
+  timetext[4] = '0' + (time & 15);
+  timetext[5] = 0;
+
+  if ((frameno & 16) == 0) {
+    //for (int i = 0; i < 4; i++) {
+    int i = rand() % 4;
+    int rnd;
+    while ((rnd = rand() % 32) > 25);
+    bigtext[i] = 'A' + rnd;
+    //}
+    bigtext[4] = 0;
+  }
+}
+
+void draw_xyxor()
+{
+  frameno++;
+
+  trazador.SetPace(0);
+  scale = 0;
+
+  //draw_marchingborder();
+  //draw_spiral();
+
+  int ox = 128 - textscale * TEXT_CHARWIDTH * XYXOR_LEN / 2 / 256;
+  int oy = 128 - textscale * TEXT_CHARHEIGHT / 2 / 256;
+  irotate(&ox, &oy, 128, 128, textangle / 5);
+  trazador.MoveTo(ox, oy);
+  //text.Str(XYXOR_WORD, textscale, textangle/5);
+
+  textangle--;
+  textscale += d_textscale;
+  if (textscale > 12 * 256 || textscale < 4 * 256) d_textscale = -d_textscale;
+
+  centre_x = 128 + isin(centreangle) / 4;
+  centre_y = 128 + icos(centreangle) / 4;
+  centreangle += 1;
+
+  if (++scale == 128) scale = 1;
+Serial.println(millis());
+  star.SetTransform(centre_x, centre_y, textscale/2, textscale/2, textangle/5);
+  star.Trace();
+  Serial.println(millis());
+
+#if 0
+  sinus.SetHalfPeriods(2);
+  scale_sin += scale_sin_d;
+  if (scale_sin == 32 || scale_sin == -32) scale_sin_d = -scale_sin_d;
+  sinus.SetTransform(128, 200, 100, scale_sin, 0);
+  sinus.Trace();
+#endif
+
+  //sinus.SetTransform(128,200, 256, scale_sin, 0);
+  //scale_sin += scale_sin_d;
+  //if (scale_sin == 128 || scale_sin == -128) scale_sin_d = -scale_sin_d;
+
+  //draw_marchingborder();
+#if 0
+  if ((textangle & 7) == 0) {
+    grid.SetTransform(128, 128, 180, 180, 0);
+    //grid.SetTransform(centre_x, centre_y, 180, 180, textangle);
+    grid.Trace();
+  }
+#endif
+
+  if (frameno % 16 == 0) {
+    time = (time & 0xff00) + bcd_increment(time & 0xff);
+    if ((time & 0xff) == 0x60) {
+      time = bcd_increment(time >> 8) << 8;
+    }
+    updateText();
+  }
+/*
+  trazador.MoveTo(60, 50);
+  text.Str(timetext, 1024, 0);
+  trazador.MoveTo(30, 100);
+  text.Str(bigtext, 2048, 0);
+  */
+}
diff --git a/src/pecado.c b/src/pecado.c
new file mode 100644
index 0000000..c1c0ba8
--- /dev/null
+++ b/src/pecado.c
@@ -0,0 +1,33 @@
+#include <avr/pgmspace.h>
+#include <inttypes.h>
+
+// sin table thanks to
+// "Sin table calculator" by Selyakov Pavel
+// http://www.meraman.com/htmls/en/sinTable.html
+const uint8_t sintab[] PROGMEM =
+{0, 3, 6, 9, 12, 16, 19, 22, 25, 28, 31, 34, 37, 40, 43, 46, 49, 51, 54, 57, 60, 63, 65, 68, 71, 73, 76, 78, 81, 83, 85, 88, 90, 92, 94, 96, 98, 100, 102, 104, 106, 107, 109, 111, 112, 113, 115, 116, 117, 118, 120, 121, 122, 122, 123, 124, 125, 125, 126, 126, 126, 127, 127, 127, 127, 127, 127, 127, 126, 126, 126, 125, 125, 124, 123, 122, 122, 121, 120, 118, 117, 116, 115, 113, 112, 111, 109, 107, 106, 104, 102, 100, 98, 96, 94, 92, 90, 88, 85, 83, 81, 78, 76, 73, 71, 68, 65, 63, 60, 57, 54, 51, 49, 46, 43, 40, 37, 34, 31, 28, 25, 22, 19, 16, 12, 9, 6, 3, 0, -3, -6, -9, -12, -16, -19, -22, -25, -28, -31, -34, -37, -40, -43, -46, -49, -51, -54, -57, -60, -63, -65, -68, -71, -73, -76, -78, -81, -83, -85, -88, -90, -92, -94, -96, -98, -100, -102, -104, -106, -107, -109, -111, -112, -113, -115, -116, -117, -118, -120, -121, -122, -122, -123, -124, -125, -125, -126, -126, -126, -127, -127, -127, -127, -127, -127, -127, -126, -126, -126, -125, -125, -124, -123, -122, -122, -121, -120, -118, -117, -116, -115, -113, -112, -111, -109, -107, -106, -104, -102, -100, -98, -96, -94, -92, -90, -88, -85, -83, -81, -78, -76, -73, -71, -68, -65, -63, -60, -57, -54, -51, -49, -46, -43, -40, -37, -34, -31, -28, -25, -22, -19, -16, -12, -9, -6, -3 };
+
+int8_t isin(uint8_t angle)
+{
+  return pgm_read_byte(&(sintab[angle]));
+}
+
+int8_t icos(uint8_t angle)
+{
+  return pgm_read_byte(&(sintab[(angle + 64) & 0377]));
+}
+
+void irotate(int16_t* x, int16_t* y, int ox, int oy, uint8_t angle)
+{
+  int sintheta = isin(angle);
+  int costheta = icos(angle);
+
+  int xc = *x - ox;
+  int yc = *y - oy;
+
+  int xr =  xc * costheta + yc * sintheta;
+  int yr = -xc * sintheta + yc * costheta;
+
+  *x = xr / 128 + ox;
+  *y = yr / 128 + oy;
+}
diff --git a/src/pecado.h b/src/pecado.h
new file mode 100644
index 0000000..c627af1
--- /dev/null
+++ b/src/pecado.h
@@ -0,0 +1,16 @@
+#pragma once
+
+#include <inttypes.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+int8_t isin(uint8_t angle);
+int8_t icos(uint8_t angle);
+void irotate(int16_t* x, int16_t* y, int ox, int oy, uint8_t angle);
+
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/src/shape.cpp b/src/shape.cpp
new file mode 100644
index 0000000..df07f73
--- /dev/null
+++ b/src/shape.cpp
@@ -0,0 +1,141 @@
+#include <inttypes.h>
+#include <avr/pgmspace.h>
+#include "shape.h"
+#include "line.h"
+#include "pecado.h"
+#include "dac.h"
+
+#define SWAP(a,b) {(a)^=(b);(b)^=(a);(a)^=(b);}
+
+void Shape::SetTransform(int ox, int oy, int scalex, int scaley, uint8_t angle)
+{
+  this->ox = ox;
+  this->oy = oy;
+  this->scalex = scalex;
+  this->scaley = scaley;
+  this->angle = angle;
+}
+
+void Shape::Trace(void)
+{
+  int x, y, z;
+
+  for (int i = 0; GetXYZ(i, &x, &y, &z) != 0; i++) {
+    x = x * scalex;  x = x / 256;
+    y = y * scaley;  y = y / 256;
+
+    irotate(&x, &y, 0, 0, angle);
+    x += ox;
+    y += oy;
+    if (i == 0 || z == 0) {
+      xyz.SetZ(0);
+      trazador.MoveTo(x, y);
+    } else {
+      xyz.SetZ(1);
+      LineTo(x, y);
+    }
+  }
+  xyz.SetZ(0);
+}
+
+void Shape::LineTo(int x, int y)
+{
+  trazador.LineTo(x, y);
+}
+
+int ContiguousShape::GetXYZ(uint8_t n, int* x, int* y, int* z)
+{
+  if (n >= npoints)
+    return 0;
+
+  *x = (int8_t) pgm_read_byte(&(data[n << 1]));
+  *y = (int8_t) pgm_read_byte(&(data[(n << 1) + 1]));
+  *z = 1;
+  return 1;
+}
+
+ContiguousShape::ContiguousShape(PGM_IP data, int npoints)
+{
+  this->data = data;
+  this->npoints = npoints;
+}
+
+
+const int8_t boxshape[] PROGMEM =
+{ -1, -1,
+  1, -1,
+  1, 1,
+  -1, 1,
+  -1, -1
+};
+
+const int8_t starshape[] PROGMEM =
+{ 9, 3,
+  -6, -8,
+  0, 10,
+  6, -8,
+  -9, 3,
+  9, 3
+};
+
+ContiguousShape box(boxshape, sizeof(boxshape) / sizeof(boxshape[0]) / 2);
+ContiguousShape star(starshape, sizeof(starshape) / sizeof(starshape[0]) / 2);
+
+void SinShape::SetHalfPeriods(uint8_t n)
+{
+  npoints = n * SINSUBDIVS;
+  step = 256 / npoints;
+}
+
+int SinShape::GetXYZ(uint8_t n, int* x, int* y, int* z)
+{
+  if (n >= npoints) return 0;
+
+  *z = 1;
+
+  *y = isin(n * (128 / SINSUBDIVS));
+  *x = n * step - 128;
+
+  return 1;
+}
+
+void SinShape::LineTo(int x, int y)
+{
+  trazador.LineTo(x, y);
+}
+
+SinShape sinus;
+
+#define GRID8
+
+int GridShape::GetXYZ(uint8_t n, int* x, int* y, int* z)
+{
+  uint8_t swap;
+#ifdef GRID8
+  // 8x8 grid: 18+18 = 36 points
+  if (n >= 36) return 0;
+  swap = n >= 18;
+  if (swap) n -= 18;
+#else
+  // 16x16 grid: 34+34 = 68 points
+  if (n >= 68) return 0;
+  swap = n >= 34;
+  if (swap) n -= 34;
+#endif
+  switch (n & 3) {
+    case 0: *x = -127; *z = 0; break;
+    case 1: *x =  127; *z = 1; break;
+    case 2: *x =  127; *z = 0; break;
+    case 3: *x = -127; *z = 1; break;
+  }
+#ifdef GRID8
+  *y = ((n >> 1) << 5) - 127;
+#else
+  *y = ((n >> 1) << 4) - 127;
+#endif
+  if (swap) SWAP(*x, *y);
+
+  return 1;
+}
+
+GridShape grid;
diff --git a/src/shape.h b/src/shape.h
new file mode 100644
index 0000000..474008a
--- /dev/null
+++ b/src/shape.h
@@ -0,0 +1,61 @@
+#pragma once
+
+#include <inttypes.h>
+#include <avr/pgmspace.h>
+
+#define PGM_IP const int8_t *
+
+class Shape 
+{
+private:
+    int scalex, scaley;
+    int angle;
+    int ox, oy;
+
+protected:
+    virtual int GetXYZ(uint8_t n, int* x, int* y, int* z) = 0;
+    virtual void LineTo(int x, int y);
+public:
+    void SetTransform(int ox, int oy, int scalex, int scaley, uint8_t angle);
+    void Trace(void);
+};
+
+class ContiguousShape : public Shape
+{
+private:
+    PGM_IP data;
+    uint8_t npoints;
+protected:
+    virtual int GetXYZ(uint8_t n, int* x, int* y, int* z);
+public:
+    ContiguousShape(PGM_IP data, int npoints);
+};
+
+extern ContiguousShape box;
+extern ContiguousShape star;
+
+#define SINSUBDIVS 8
+
+class SinShape : public Shape
+{
+private:
+    uint8_t npoints;
+    uint8_t step;
+
+public:
+    void SetHalfPeriods(uint8_t n); 
+
+protected:
+    virtual int GetXYZ(uint8_t n, int* x, int* y, int* z);
+    virtual void LineTo(int x, int y);
+};
+
+extern SinShape sinus;
+
+class GridShape : public Shape
+{
+protected:
+    virtual int GetXYZ(uint8_t n, int* x, int* y, int* z);
+};
+
+extern GridShape grid;
diff --git a/src/text.cpp b/src/text.cpp
new file mode 100644
index 0000000..0cb68b6
--- /dev/null
+++ b/src/text.cpp
@@ -0,0 +1,54 @@
+#include <inttypes.h>
+#include <avr/pgmspace.h>
+
+#include "line.h"
+#include "pecado.h"
+#include "text.h"
+#include "dac.h"
+
+extern "C" PGM_P charset0[256];
+
+Text text;
+
+void Text::Char(uint8_t c, int ox, int oy, int16_t scale)
+{
+  int x = ox;
+  int y = oy;
+  int blanco;
+  PGM_P coffs;
+  uint8_t encoded;
+
+  coffs = (PGM_P) pgm_read_word(&(charset0[c - 0x20]));
+
+  for (int i = 0; (encoded = pgm_read_byte(coffs)); coffs++, i++) {
+    x = ox + (scale * ((encoded >> 4) & 007)) / 256;
+    y = oy + (scale * ((encoded & 017) - 4)) / 256;
+    blanco = encoded & 0200 ? 1 : 0;
+
+    irotate(&x, &y, ox, oy, text_angle);
+    if (i == 0 || !blanco) {
+      xyz.SetZ(0);
+      trazador.MoveTo(x, y);
+    }
+    else {
+      xyz.SetZ(1);
+      trazador.LineTo(x, y);
+    }
+  }
+
+
+  xyz.SetZ(0);
+
+  x = ox + (6 * scale) / 256;
+  y = oy;
+  irotate(&x, &y, ox, oy, text_angle);
+  trazador.MoveTo(x, y);
+}
+
+void Text::Str(const char *s, int16_t scale, uint8_t angle)
+{
+  text_angle = angle;
+  for (; *s != 0; s++) {
+    Char(*s, trazador.X, trazador.Y, scale);
+  }
+}
diff --git a/src/text.h b/src/text.h
new file mode 100644
index 0000000..0254b93
--- /dev/null
+++ b/src/text.h
@@ -0,0 +1,16 @@
+#pragma once
+
+#include <inttypes.h>
+
+#define TEXT_CHARWIDTH 6
+#define TEXT_CHARHEIGHT 9
+
+class Text {
+private:
+    uint8_t text_angle;
+public:
+    void Char(uint8_t c, int ox, int oy, int16_t scale);
+    void Str(const char *s, int16_t scale, uint8_t angle);
+};
+
+extern Text text;
diff --git a/src/util.c b/src/util.c
new file mode 100644
index 0000000..95e49c8
--- /dev/null
+++ b/src/util.c
@@ -0,0 +1,54 @@
+#include <inttypes.h>
+#include "util.h"
+
+uint8_t frombcd(uint8_t x) {
+  return _frombcd(x);
+}
+
+uint8_t month_length(uint8_t m, uint8_t leap) {
+  uint8_t knuckle = m < 8 ? (m & 1) == 1 : (m & 1) == 0;
+  return knuckle ? 31 : m != 2 ? 30 : leap ? 29 : 28;
+}
+
+uint8_t days_in_month_bcd(uint8_t year, uint8_t month) {
+  uint8_t y = frombcd(year);//(year & 7) + ((year & 070)>>3) * 10;
+  uint8_t m = frombcd(month);//(month & 7) + ((month & 070)>>3) * 10;
+  uint8_t leap = y % 4 == 0; // non y2k1-compliant, but should be correct for the next 91 years or so :)
+
+  uint8_t knuckle = m < 8 ? (m & 1) == 1 : (m & 1) == 0;
+  return knuckle ? 0x31 : m != 2 ? 0x30 : leap ? 0x29 : 0x28;
+}
+
+
+uint8_t bcd_increment(uint8_t x) {
+  x++;
+  if ((x & 0x0f) >= 0x0a)
+    x += 0x10 - 0x0a;
+  if (x >= 0xa0)
+    x = 0;
+  return x;
+}
+
+
+uint8_t day_of_week(uint8_t y, uint8_t m, uint8_t d) {
+  uint8_t leap = y % 4 == 0;
+  uint16_t centurydays = 6 + y * 365 + (y + 3) / 4; // year 2000 started on Saturday
+  for (; --m >= 1; ) {
+    centurydays += month_length(m, leap);
+  }
+
+  centurydays += d - 1;
+
+  return (centurydays % 7);
+}
+
+uint16_t tobcd16(uint16_t x) {
+  uint16_t y;
+
+  y  = x % 10;        x /= 10;
+  y |= (x % 10) << 4;   x /= 10;
+  y |= (x % 100) << 8;  x /= 10;
+  y |= 0xf000;
+
+  return y;
+}
diff --git a/src/util.h b/src/util.h
new file mode 100644
index 0000000..f1dd829
--- /dev/null
+++ b/src/util.h
@@ -0,0 +1,78 @@
+/// \file
+/// \brief Utilities
+///
+/// Bit settings. BCD conversions. Calendar.
+///
+#pragma once
+
+#define BV2(a,b) (_BV(a)|_BV(b))
+#define BV3(a,b,c) (_BV(a)|_BV(b)|_BV(c))
+#define BV4(a,b,c,d) (_BV(a)|_BV(b)|_BV(c)|_BV(d))
+#define BV5(a,b,c,d,e) (_BV(a)|_BV(b)|_BV(c)|_BV(d)|_BV(e))
+#define BV6(a,b,c,d,e,f) (_BV(a)|_BV(b)|_BV(c)|_BV(d)|_BV(e)|_BV(f))
+#define BV7(a,b,c,d,e,f,g) (_BV(a)|_BV(b)|_BV(c)|_BV(d)|_BV(e)|_BV(f)|_BV(g))
+
+/// Make BCD time from 2 bytes
+#define maketime(hh,mm) (((hh) << 8) + (mm))
+
+/// Convert to binary from BCD representation 
+/// \see frombcd
+#define _frombcd(x) ((x & 017) + (((x) & 0160)>>4) * 10)
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/// Convert to binary from BCD representation as a function.
+/// \see _frombcd
+uint8_t frombcd(uint8_t);
+
+uint16_t tobcd16(uint16_t);
+
+/// Return BCD count of days in month for given BCD year and month.
+/// Valid only for years 2000-2099.
+uint8_t days_in_month_bcd(uint8_t year, uint8_t month);
+
+/// Increment BCD value by 1
+uint8_t bcd_increment(uint8_t x);
+
+/// Get day of week, 0 = Sunday. Input parameters are binary (not BCD)
+uint8_t day_of_week(uint8_t y, uint8_t m, uint8_t d);
+
+/// Set blinkmode
+void blinkmode_set(uint8_t mode);
+
+/// Get blinkmode
+uint8_t blinkmode_get();
+
+/// Called every blink, unless NULL
+//void (*blinkhandler)(uint8_t);
+
+/// Sets fading mode and speed
+/// \see _fademode
+void fade_set(uint8_t mode);
+
+void fadeto(uint16_t t);
+
+uint16_t get_display_value();
+
+/// Cycle display modes
+/// \see _displaymode
+void mode_next();
+
+/// get display mode
+/// \see _displaymode
+uint8_t mode_get();
+
+void savingmode_set(uint8_t s);
+uint8_t savingmode_get();
+void savingmode_next();
+
+// see dot blinking mode
+// \see _dotmode
+void dotmode_set(uint8_t mode);
+
+#ifdef __cplusplus
+}
+#endif
+