diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json
index 6449072..103ee70 100644
--- a/.vscode/c_cpp_properties.json
+++ b/.vscode/c_cpp_properties.json
@@ -12,7 +12,7 @@
 			"intelliSenseMode": "linux-gcc-x64",
 			"browse": {
 				"path": [
-					"/usr/include",
+					"/usr/include/**",
 					"${workspaceFolder}/**"
 				]
 			}
diff --git a/Makefile b/Makefile
index 2ddc8a3..7ee0810 100644
--- a/Makefile
+++ b/Makefile
@@ -2,17 +2,30 @@ TARGETNAME = inverter
 BUILDFOLDER = ./build/
 TARGET = $(BUILDFOLDER)$(TARGETNAME)
 
+TEST_TARGETNAME = test
+TEST_BUILDFOLDER = ./test/build/
+TEST_TARGET = $(TEST_BUILDFOLDER)$(TEST_TARGETNAME)
+
 INC = -Isrc/include -I/usr/include -I/usr/include/libftdi1
-CFLAGS = -Wall -Wpedantic -Wextra -Os 
+CFLAGS = -Wall -Wpedantic -Wextra -g
 CPPFLAGS = 
 
 SRC = $(wildcard src/*.c)
+TEST_SRC = $(filter-out src/main.c, $(SRC) $(wildcard test/*.c))
 
-all: $(TARGET)
+$(TARGETNAME): $(TARGET)
 
 $(TARGET): $(SRC)
 	mkdir -p build
-	gcc -o $@ $^ $(CPPFLAGS) $(CFLAGS) $(INC) -lm -lftdi1 -g
+	cc -o $@ $^ $(CPPFLAGS) $(CFLAGS) $(INC) -lm -lftdi1 
+
+$(TEST_TARGETNAME): $(TEST_TARGET)
+
+$(TEST_TARGET): $(TEST_SRC)
+	echo $(TEST_SRC)
+	mkdir -p $(TEST_BUILDFOLDER)
+	cc -o $@ $^ $(CPPFLAGS) $(CFLAGS) $(INC) -lm -lftdi1 -lcunit
 
 clean:
-	rm -fr ./src/*.o ./build/
+	rm -fr ./src/*.o ./build/*
+	rm -fr $(TEST_BUILDFOLDER)*
diff --git a/src/bridge_utils.c b/src/bridge_utils.c
index ccd1cab..21e1dd8 100644
--- a/src/bridge_utils.c
+++ b/src/bridge_utils.c
@@ -39,45 +39,45 @@ sm_t bridge_statemachine = {
 void brdg_BothGndOn(void)
 {
 	/* Turn both GND transistors on */
-	setPin(PB6);
-	setPin(PB4);
+	setPin(D0);
+	setPin(D2);
 
 	/* Turn both PWR transistors off */
-	unsetPin(PB7);
-	unsetPin(PB5);
+	unsetPin(D3);
+	unsetPin(D1);
 }
 
 void brdg_Hb1GndOn(void)
 {
 	/* We turn GND of half-bridge 1 on */
-	setPin(PB6);
+	setPin(D0);
 
 	/* And then we turn GND of half-bridge 2 off */
-	unsetPin(PB4);
+	unsetPin(D2);
 }
 
 void brdg_Hb1PowerOn(void)
 {
-	setPin(PB7);
+	setPin(D3);
 }
 
 void brdg_Hb1PowerOff(void)
 {
-	unsetPin(PB7);
+	unsetPin(D3);
 }
 
 void brdg_Hb2GndOn(void)
 {
-	setPin(PB4);
-	unsetPin(PB6);
+	setPin(D0);
+	unsetPin(D2);
 }
 
 void brdg_Hb2PowerOn(void)
 {
-	setPin(PB5);
+	setPin(D1);
 }
 
 void brdg_Hb2PowerOff(void)
 {
-	unsetPin(PB5);
+	unsetPin(D1);
 }
diff --git a/src/include/pins.h b/src/include/pins.h
index b0e2fae..4766f00 100644
--- a/src/include/pins.h
+++ b/src/include/pins.h
@@ -1,9 +1,9 @@
 #include <inttypes.h>
 
-#define PB6 1
-#define PB4 2
-#define PB5 3
-#define PB7 4
+#define D0 0
+#define D1 1
+#define D2 2
+#define D3 3
 
 void togglePin(unsigned int pin);
 void unsetPin(unsigned int pin);
diff --git a/src/include/timer_utils.h b/src/include/timer_utils.h
index 3f5b189..93d0aa7 100644
--- a/src/include/timer_utils.h
+++ b/src/include/timer_utils.h
@@ -14,4 +14,4 @@ typedef struct
 void tmr_initTimer(tmr_ctx *ctx);
 void tmr_handleTimerOverflow(union sigval);
 int tmr_callEvery(void (*func)(void), long nsec);
-void tmr_add_ns_to_current_time(struct itimerspec *current, long usecs);
\ No newline at end of file
+void tmr_add_ns_to_current_time(struct itimerspec *current, long nsecs);
\ No newline at end of file
diff --git a/src/main.c b/src/main.c
index 6096d3f..21e9db2 100644
--- a/src/main.c
+++ b/src/main.c
@@ -12,43 +12,32 @@ static uint64_t counter = 0;
 void test1(void)
 {
 	printf("%lu\n", counter);
+	fflush(stdout);
 	counter++;
 }
 
 void test2(void)
 {
 	printf("This is test2\n");
+	fflush(stdout);
 }
 
 void togglePin1(void)
 {
-	printf("toggling pin\n");
-	setPin(0);
+	togglePin(D0);
 }
 
 int main()
 {
 	printf("Program started\n");
 
-	if (tmr_callEvery(test1, 1 * SEC))
-	{
-		printf("Error starting timer for test\n");
-		return EXIT_FAILURE;
-	}
-
-	if (tmr_callEvery(test2, 1 * SEC))
-	{
-		printf("Error starting timer for test\n");
-		return EXIT_FAILURE;
-	}
-
 	if (init_ftdi())
 	{
 		printf("Error initializing ftdi\n");
 		return EXIT_FAILURE;
 	}
 
-	if (tmr_callEvery(togglePin1, 1 * SEC))
+	if (tmr_callEvery(togglePin1, 10 * USEC))
 	{
 		printf("Error starting timer for togglePin\n");
 		return EXIT_FAILURE;
diff --git a/src/timer_utils.c b/src/timer_utils.c
index bfa021e..9c9a53c 100644
--- a/src/timer_utils.c
+++ b/src/timer_utils.c
@@ -10,7 +10,8 @@
 int tmr_registry_index = 0;
 tmr_ctx *tmr_registry[10];
 
-void tmr_add_to_registry(tmr_ctx* ctx) {
+void tmr_add_to_registry(tmr_ctx *ctx)
+{
 	tmr_registry[tmr_registry_index] = ctx;
 	tmr_registry_index++;
 }
@@ -44,7 +45,8 @@ void tmr_handleTimerOverflow(sigval_t ctx_par)
 
 int tmr_callEvery(void (*func)(void), long nsec)
 {
-	tmr_ctx *tmr = malloc(sizeof(tmr_ctx));
+	tmr_ctx *tmr = calloc(1,sizeof(tmr_ctx));
+	// tmr_ctx *tmr = malloc(sizeof(tmr_ctx));
 
 	tmr_add_to_registry(tmr);