diff --git a/quad/library.mk b/quad/library.mk
index ab20cd9b1880cb08e6ca384b113206e3bdf6813c..dc8a34d367c193b42472cc27f58a1a8a3f79451f 100644
--- a/quad/library.mk
+++ b/quad/library.mk
@@ -51,5 +51,5 @@ $(OBJDIR):
 $(LIBDIR):
 	mkdir $(LIBDIR)
 
-$(TESTBIN): $(TESTOBJECTS) $(OBJECTS)
+$(TESTBIN): $(TESTOBJECTS) $(OBJECTS) | default
 	$(GCC) -o $(TESTBIN) $^ -I$(INCDIR) -L$(LIBDIR) $(REQLIBS)
diff --git a/quad/src/queue/queue.c b/quad/src/queue/queue.c
index 3eb19fc2d7c16c1af7b61cb0dab63973c1f2d9c1..243975b0a3f14a77e811ecb33249528ff98ba5c4 100644
--- a/quad/src/queue/queue.c
+++ b/quad/src/queue/queue.c
@@ -1,28 +1,28 @@
 #include "queue.h"
 
-struct Queue queue_create(unsigned int max_size) {
-  struct Queue q;
-  q.buff = malloc(sizeof(char) * (max_size + 1));
-  q.max_size = max_size + 1;
-  q.start = max_size;
-  q.end = 0;
-  return q;
-}
-
-int queue_init(struct Queue *q, unsigned int max_size) {
-  q->buff = malloc(sizeof(unsigned char) * (max_size + 1));
-  if (q->buff == NULL) return -1;
-  q->max_size = max_size + 1; // required for start/end pointers
+int queue_init(struct Queue *q, volatile unsigned char *buff, unsigned int max_size) {
+  q->buff = buff;
+  q->max_size = max_size + 1;
   q->start = max_size;
   q->end = 0;
   return 0;
 }
 
-void queue_destroy(struct Queue *q) {
-  free(q->buff);
+struct Queue * queue_malloc(unsigned int max_size) {
+  unsigned char *buff = malloc(sizeof(unsigned char) * (max_size + 1));
+  if (buff == NULL) return NULL;
+  struct Queue *q = malloc(sizeof(struct Queue));
+  if (q == NULL) return NULL;
+  queue_init(q, buff, max_size);
+  return q;
+}
+
+void queue_free(struct Queue *q) {
+  free((unsigned char *) q->buff);
+  free(q);
 }
 
-int queue_add(volatile struct Queue *q, unsigned char c) {
+int queue_add(struct Queue *q, unsigned char c) {
   if (queue_full(q)) {
     return -1;
   }
@@ -32,7 +32,7 @@ int queue_add(volatile struct Queue *q, unsigned char c) {
   return 0;
 }
 
-int queue_remove(volatile struct Queue *q, unsigned char *c) {
+int queue_remove(struct Queue *q, unsigned char *c) {
   if (queue_empty(q)) {
     return -1;
   }
@@ -42,21 +42,21 @@ int queue_remove(volatile struct Queue *q, unsigned char *c) {
   return 0;
 }
 
-int queue_size(volatile struct Queue *q) {
+int queue_size(struct Queue *q) {
   return (q->max_size + q->end - q->start - 1) % q->max_size;
 }
 
-int queue_full(volatile struct Queue *q) {
+int queue_full(struct Queue *q) {
   return q->start == q->end;;
 }
 
-int queue_empty(volatile struct Queue *q) {
+int queue_empty(struct Queue *q) {
   return queue_size(q) == 0;
 }
 
 void queue_print(struct Queue *q) {
   int i;
-  printf("buffer contents:\n");
+  printf("buffer: (size: %d, full: %d, empty: %d)\n", queue_size(q), queue_full(q), queue_empty(q));
   for (i = 0; i < q->max_size; i += 1) {
     char c = (char) q->buff[i];
     if (c == 0) c = '-';
diff --git a/quad/src/queue/queue.h b/quad/src/queue/queue.h
index 8e34618446b79f6de2bdb1801c2db7168287cbb8..1adf670ba24877b93087aba40470057bfcc766e8 100644
--- a/quad/src/queue/queue.h
+++ b/quad/src/queue/queue.h
@@ -5,21 +5,21 @@
 #include <stdio.h>
 
 struct Queue {
-  unsigned char *buff;
+  volatile unsigned char *buff;
   unsigned int max_size;
   unsigned int start;
   unsigned int end;
 };
 
 
-struct Queue queue_create(unsigned int max_size);
-int queue_init(struct Queue *q, unsigned int max_size);
-void queue_destory(struct Queue *q);
-int queue_add(volatile struct Queue *q, unsigned char c);
-int queue_remove(volatile struct Queue *q, unsigned char *c);
-int queue_size(volatile struct Queue *q);
-int queue_full(volatile struct Queue *q);
-int queue_empty(volatile struct Queue *q);
+int queue_init(struct Queue *q, volatile unsigned char *buff, unsigned int max_size);
+struct Queue * queue_malloc(unsigned int max_size);
+void queue_free(struct Queue *q);
+int queue_add(struct Queue *q, unsigned char c);
+int queue_remove(struct Queue *q, unsigned char *c);
+int queue_size(struct Queue *q);
+int queue_full(struct Queue *q);
+int queue_empty(struct Queue *q);
 void queue_print(struct Queue *q);
 
 #endif
diff --git a/quad/src/queue/test/test_queue.c b/quad/src/queue/test/test_queue.c
index 58b9edc8f9b2deb9136cd23fa1155ab0b65b87e0..02328d1f90994daa884c28cea06db4c63cca0d9a 100644
--- a/quad/src/queue/test/test_queue.c
+++ b/quad/src/queue/test/test_queue.c
@@ -1,143 +1,170 @@
 #include "test.h"
 #include "queue.h"
 
+int test_free() {
+  struct Queue *q = queue_malloc(4);
+  queue_free(q);
+  return 0;
+}
+
 int test_add() {
-  puts("---");
-  puts("test_add");
+  struct Queue *q = queue_malloc(4);
+  unsigned char c;
+  test_assert(!queue_add(q, 'a'));
+  test_assert(!queue_add(q, 'b'));
+  test_assert(!queue_add(q, 'c'));
+  test_assert(!queue_add(q, 'd'));
+  test_assert(queue_add(q, 'x'));
+  queue_print(q);
+
+  queue_remove(q, &c);
+  queue_remove(q, &c);
+  test_assert(!queue_add(q, 'e'));
+  test_assert(!queue_add(q, 'f'));
+  test_assert(queue_add(q, 'x'));
+  queue_print(q);
+
+  return 0;
+}
+
+int test_remove() {
   int failed = 0;
-  struct Queue *q = malloc(sizeof(struct Queue));
-  queue_init(q, 5);
-  puts("initialized");
+  struct Queue *q = queue_malloc(4);
   unsigned char c;
-  failed = failed || queue_add(q, 'a');
+  queue_add(q, 'x');
+  queue_add(q, 'x');
+  test_assert(!queue_remove(q, &c));
+  test_assert((c == 'x'));
+  test_assert(!queue_remove(q, &c));
+  test_assert((c == 'x'));
+  test_assert(queue_remove(q, &c));
+  queue_print(q);
+
+  queue_add(q, 'a');
+  queue_add(q, 'b');
+  queue_add(q, 'c');
+  queue_add(q, 'd');
+  queue_add(q, 'x');
+  test_assert(!queue_remove(q, &c));
+  test_assert((c == 'a'));
+  test_assert(!queue_remove(q, &c));
+  test_assert((c == 'b'));
+  test_assert(!queue_remove(q, &c));
+  test_assert((c == 'c'));
+  test_assert(!queue_remove(q, &c));
+  test_assert((c == 'd'));
+  test_assert(queue_remove(q, &c));
+
   return failed;
 }
 
+
 int test_size() {
-  puts("---");
-  puts("test_size");
   int failed = 0;
-  struct Queue *q = malloc(sizeof(struct Queue));
-  queue_init(q, 4);
+  struct Queue *q = queue_malloc(4);
   unsigned char c;
-  failed = failed || queue_size(q) != 0;
-  printf("size: %d\n", queue_size(q));
+
+  test_assert(queue_size(q) == 0);
   queue_print(q);
 
   queue_add(q, 'a');
-
-  printf("size: %d\n", queue_size(q));
+  test_assert(queue_size(q) == 1);
   queue_print(q);
-  failed = failed || queue_size(q) != 1;
 
   queue_add(q, 'b');
-
-  printf("size: %d\n", queue_size(q));
+  test_assert(queue_size(q) == 2);
   queue_print(q);
-  failed = failed || queue_size(q) != 2;
 
   queue_add(q, 'c');
-
-  printf("size: %d\n", queue_size(q));
+  test_assert(queue_size(q) == 3);
   queue_print(q);
-  failed = failed || queue_size(q) != 3;
 
   queue_add(q, 'd');
-
-  printf("size: %d\n", queue_size(q));
+  test_assert(queue_size(q) == 4);
   queue_print(q);
-  failed = failed || queue_size(q) != 4;
 
   queue_remove(q, &c);
   queue_remove(q, &c);
-
-  printf("size: %d\n", queue_size(q));
+  test_assert(queue_size(q) == 2);
   queue_print(q);
-  failed = failed || queue_size(q) != 2;
 
   queue_add(q, 'e');
-
-  printf("size: %d\n", queue_size(q));
+  test_assert(queue_size(q) == 3);
   queue_print(q);
-  failed = failed || queue_size(q) != 3;
 
   return failed;
 }
 
-int test_full_when_full() {
-  puts("---");
-  puts("test_full_when_full");
+int test_full() {
   int failed = 0;
-  struct Queue *q = malloc(sizeof(struct Queue));
-  queue_init(q, 4);
+  struct Queue *q = queue_malloc(4);
   unsigned char c;
-  queue_add(q, 'a');
-  queue_add(q, 'b');
-  queue_add(q, 'c');
-  queue_add(q, 'd');
-  failed = failed || !queue_full(q);
-  return failed;
-}
 
-int test_add_when_full() {
-  puts("---");
-  puts("test_add_when_full");
-  int failed = 0;
-  struct Queue *q = malloc(sizeof(struct Queue));
-  queue_init(q, 4);
-  unsigned char c;
+  test_assert(!queue_full(q));
+  queue_print(q);
+
   queue_add(q, 'a');
   queue_add(q, 'b');
   queue_add(q, 'c');
-  queue_add(q, 'd');
-  queue_print(q);
-  failed = failed || !queue_add(q, 'x');
-  queue_remove(q, &c);
+  test_assert(!queue_full(q));
   queue_print(q);
-  failed = failed || !(c == 'a');
-  queue_remove(q, &c);
+
+  queue_add(q, 'd');
+  test_assert(queue_full(q));
   queue_print(q);
-  failed = failed || !(c == 'b');
+
   queue_remove(q, &c);
+  test_assert(!queue_full(q));
   queue_print(q);
-  failed = failed || !(c == 'c');
+
   queue_remove(q, &c);
+  queue_add(q, 'x');
+  queue_add(q, 'x');
+  test_assert(queue_full(q));
   queue_print(q);
-  failed = failed || !(c == 'd');
+
   return failed;
 }
 
-int test_add_over_edge() {
+int test_empty() {
   int failed = 0;
-  struct Queue *q = malloc(sizeof(struct Queue));
-  queue_init(q, 4);
+  struct Queue *q = queue_malloc(4);
   unsigned char c;
-  queue_add(q, 'x');
-  queue_add(q, 'x');
-  queue_remove(q, &c);
-  queue_remove(q, &c);
+
+  test_assert(queue_empty(q));
+  queue_print(q);
+
   queue_add(q, 'a');
   queue_add(q, 'b');
   queue_add(q, 'c');
+  test_assert(!queue_empty(q));
+  queue_print(q);
+
   queue_add(q, 'd');
+  test_assert(!queue_empty(q));
   queue_print(q);
-  failed = failed || !queue_add(q, 'x');
+
   queue_remove(q, &c);
-  failed = failed || !(c == 'a');
+  test_assert(!queue_empty(q));
+  queue_print(q);
+
   queue_remove(q, &c);
-  failed = failed || !(c == 'b');
   queue_remove(q, &c);
-  failed = failed || !(c == 'c');
+  test_assert(!queue_empty(q));
+
   queue_remove(q, &c);
-  failed = failed || !(c == 'd');
+  test_assert(queue_empty(q));
+
   return failed;
 }
 
+
 int main() {
+  test(test_free, "test_free");
   test(test_add, "test_add");
+  test(test_remove, "test_remove");
   test(test_size, "test_size");
-  test(test_full_when_full, "test_full_when_full");
-  test(test_add_when_full, "test_add_when_full");
-  test(test_add_over_edge, "test_add_over_edge");
+  test(test_full, "test_full");
+  test(test_empty, "test_empty");
   return test_summary();
 }
diff --git a/quad/src/test/test.c b/quad/src/test/test.c
index dc2ef00efc660656444a502f5e802873e4aee80e..c571c4a91986c3acc042de6e997e1568f0eff395 100644
--- a/quad/src/test/test.c
+++ b/quad/src/test/test.c
@@ -3,6 +3,7 @@
 static int num_tests = 0;
 static struct Test tests[128];
 static int longest_test_name = 0;
+static int num_assertions = 0;
 
 void test(int (*function)(), char *test_name) {
   int test_name_length = strlen(test_name);
@@ -13,6 +14,8 @@ void test(int (*function)(), char *test_name) {
   pid_t pid = fork();
   if (pid == 0) {
     // test process
+    puts("----------------------------------------");
+    printf("#%3d: %s\n", num_tests + 1, test_name);
     int exit_status = function();
     exit(exit_status);
   } else {
@@ -40,10 +43,18 @@ void test(int (*function)(), char *test_name) {
   }
 }
 
+void test_assert(int success) {
+  num_assertions += 1;
+  if (!success) {
+    printf("FAILED assertion #%d\n", num_assertions);
+    exit(1);
+  }
+}
+
 int test_summary() {
   unsigned char at_least_one_test_failed = 0;
   int num_failed = 0;
-  puts("---------------------------------");
+  puts("--------------------------------------------------------------------------------");
   puts("Test results:");
   puts("");
   int i = 0;
@@ -54,7 +65,7 @@ int test_summary() {
   }
   puts("");
   printf("Total: %3d of %-3d tests passed\n", num_tests - num_failed, num_tests);
-  puts("---------------------------------");
+  puts("--------------------------------------------------------------------------------");
   num_tests = 0;
   longest_test_name = 0;
   return at_least_one_test_failed;
diff --git a/quad/src/test/test.h b/quad/src/test/test.h
index 862769fc347187b3b7e6b408dab7b501d8388015..4fe3ab12e81839d9a6cb3c0d0ee3f2e75a2b8ecc 100644
--- a/quad/src/test/test.h
+++ b/quad/src/test/test.h
@@ -14,6 +14,7 @@ struct Test {
 };
 
 void test(int (*)(), char *);
+void test_assert(int success);
 int test_summary();
 
 #endif
diff --git a/quad/xsdk_workspace/modular_quad_pid/src/hw_impl_zybo_uart.c b/quad/xsdk_workspace/modular_quad_pid/src/hw_impl_zybo_uart.c
index 263e4ad9e7ded24a3f9894d847337b2dc0135370..9d87f64446063e004cbc79c8161f10e9e3fa370e 100644
--- a/quad/xsdk_workspace/modular_quad_pid/src/hw_impl_zybo_uart.c
+++ b/quad/xsdk_workspace/modular_quad_pid/src/hw_impl_zybo_uart.c
@@ -9,7 +9,8 @@ int SetupInterruptSystem(XUartPs *UartInstancePtr, u16 UartIntrId, Xil_Exception
 void uart_interrupt_handler(XUartPs *InstancePtr);
 
 // Queue has to be global to be accessible to ISR
-static volatile struct Queue queue;
+static struct Queue queue;
+static volatile unsigned char buff[MAX_UART_BUFFER_SIZE];
 static XScuGic xscugic;
 
 int zybo_uart_reset(struct UARTDriver *self) {
@@ -19,7 +20,7 @@ int zybo_uart_reset(struct UARTDriver *self) {
     if (self->state == NULL) {
       return -1;
     }
-    queue = queue_create(MAX_UART_BUFFER_SIZE);
+    queue = queue_init(&queue, buff, MAX_UART_BUFFER_SIZE);
   }
 
   XUartPs *inst = self->state;;