diff --git a/quad/computation_graph/.gitignore b/quad/computation_graph/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..0b0b67765dcc2ad33a6de329c55646c45f5e672d
--- /dev/null
+++ b/quad/computation_graph/.gitignore
@@ -0,0 +1,3 @@
+bin/
+build/
+computation_graph
diff --git a/quad/computation_graph/src/main.c b/quad/computation_graph/src/main.c
index c54494acbe5a38e9884c6fcd390f34a8d2970a36..be0e8ee775fd4b8900bb41724a8431ab6384ac11 100644
--- a/quad/computation_graph/src/main.c
+++ b/quad/computation_graph/src/main.c
@@ -1,9 +1,9 @@
 #include <stdio.h>
 #include "computation_graph.h"
-#include "node_add.h"
-#include "node_mult.h"
-#include "node_constant.h"
-#include "node_gain.h"
+#include "graph_blocks/node_add.h"
+#include "graph_blocks/node_mult.h"
+#include "graph_blocks/node_constant.h"
+#include "graph_blocks/node_gain.h"
 #include "tests.h"
 
 int main() {
diff --git a/quad/computation_graph/src/tests.c b/quad/computation_graph/src/tests.c
index 8118914cf89dea3a085ccdd0c939b56f9e1a1307..2f120a2693f4b2ee8116d192b4494299f93f44a8 100644
--- a/quad/computation_graph/src/tests.c
+++ b/quad/computation_graph/src/tests.c
@@ -33,7 +33,8 @@ int graph_test_one_add() {
     graph_set_param_val(graph, cblock4, CONST_SET, 4);
     graph_set_source(graph, block, ADD_SUMMAND1, cblock3, CONST_VAL);
     graph_set_source(graph, block, ADD_SUMMAND2, cblock4, CONST_VAL);
-    graph_compute_node(graph, block);
+    int to_compute_for[1] = {block};
+    graph_compute_nodes(graph, to_compute_for, 1);
     double result = graph_get_output(graph, block, ADD_SUM);
     return nequal(result, 7);
 }
@@ -45,7 +46,8 @@ int graph_test_circular_runs() {
     int gain2 = graph_add_node_gain(graph, "gain2");
     graph_set_source(graph, gain2, GAIN_INPUT, gain1, GAIN_RESULT);
     graph_set_source(graph, gain1, GAIN_INPUT, gain2, GAIN_RESULT);
-    graph_compute_node(graph, gain2);
+    int to_compute_for[1] = {gain2};
+    graph_compute_nodes(graph, to_compute_for, 1);
     // If no infinite loop, then success. Value is undefined for circular graphs
     return 0;
 }
@@ -66,12 +68,13 @@ int graph_test_accumulator() {
     int acum_b = graph_add_node_accum(graph, "accumulator");
     graph_set_source(graph, acum_b, ACCUM_IN, cblock, CONST_VAL);
 
+    int to_compute_for[1] = {acum_b};
     graph_set_param_val(graph, cblock, CONST_SET, 3);
-    graph_compute_node(graph, acum_b);
+    graph_compute_nodes(graph, to_compute_for, 1);
     graph_set_param_val(graph, cblock, CONST_SET, 8);
-    graph_compute_node(graph, acum_b);
+    graph_compute_nodes(graph, to_compute_for, 1);
     graph_set_param_val(graph, cblock, CONST_SET, -2);
-    graph_compute_node(graph, acum_b);
+    graph_compute_nodes(graph, to_compute_for, 1);
 
     double result = graph_get_output(graph, acum_b, ACCUMULATED);
     if (nequal(result, 9)) {
@@ -83,7 +86,8 @@ int graph_test_accumulator() {
     int gain_b = graph_add_node_gain(graph, "Gain");
     graph_set_param_val(graph, gain_b, GAIN_GAIN, 1);
     graph_set_source(graph, gain_b, GAIN_INPUT, acum_b, ACCUMULATED);
-    graph_compute_node(graph, gain_b);
+    to_compute_for[0] = gain_b;
+    graph_compute_nodes(graph, to_compute_for, 1);
     result = graph_get_output(graph, gain_b, GAIN_RESULT);
     if (nequal(result, -2)) {
         printf("graph_test_accumulator failed on step 2\n");
@@ -106,7 +110,8 @@ int graph_test_single_run() {
     graph_set_source(graph, add_block, ADD_SUMMAND1, acum_b, ACCUMULATED);
     graph_set_source(graph, add_block, ADD_SUMMAND2, acum_b, ACCUMULATED);
 
-    graph_compute_node(graph, add_block);
+    int to_compute_for[1] = {add_block};
+    graph_compute_nodes(graph, to_compute_for, 1);
     double result = graph_get_output(graph, add_block, ADD_SUM);
     return nequal(result, 4);
 }
@@ -122,14 +127,16 @@ int graph_test_reset_rules() {
 
     graph_set_source(graph, gain1, GAIN_INPUT, acum_b, ACCUMULATED);
     graph_set_source(graph, acum_b, ACCUM_IN, cblock, CONST_VAL);
-    graph_compute_node(graph, gain1);
+    int to_compute_for[1] = {gain1};
+    graph_compute_nodes(graph, to_compute_for, 1);
     // state of acum_b is now 5
 
     int gain2 = graph_add_node_gain(graph, "gain2");
     graph_set_param_val(graph, gain2, GAIN_GAIN, 1);
     // Connect gain 2, and accumulator should not get reset
     graph_set_source(graph, gain2, GAIN_INPUT, acum_b, ACCUMULATED);
-    graph_compute_node(graph, gain2);
+    to_compute_for[0] = gain2;
+    graph_compute_nodes(graph, to_compute_for, 1);
     double result = graph_get_output(graph, gain2, GAIN_RESULT);
     return nequal(result, 10);
 }
@@ -138,6 +145,7 @@ int graph_test_self_loop() {
     struct computation_graph *graph = create_graph();
     int gain1 = graph_add_node_gain(graph, "gain1");
     graph_set_source(graph, gain1, GAIN_INPUT, gain1, GAIN_RESULT);
-    graph_compute_node(graph, gain1);
+    int to_compute_for[1] = {gain1};
+    graph_compute_nodes(graph, to_compute_for, 1);
     return 0;
-}
\ No newline at end of file
+}
diff --git a/quad/computation_graph/src/tests.h b/quad/computation_graph/src/tests.h
index 9452bb525fc8984e927d5de96259588c6718498b..b30b7d806b88f3811b74f264742d4094cd4387c8 100644
--- a/quad/computation_graph/src/tests.h
+++ b/quad/computation_graph/src/tests.h
@@ -6,11 +6,11 @@
 #define COMPUTATION_GRAPH_TESTS_H
 
 #include "computation_graph.h"
-#include "node_add.h"
-#include "node_mult.h"
-#include "node_constant.h"
-#include "node_gain.h"
-#include "node_accumulator.h"
+#include "graph_blocks/node_add.h"
+#include "graph_blocks/node_mult.h"
+#include "graph_blocks/node_constant.h"
+#include "graph_blocks/node_gain.h"
+#include "graph_blocks/node_accumulator.h"
 #include <math.h>
 
 int graph_run_tests();
diff --git a/quad/computation_graph/test/Makefile b/quad/computation_graph/test/Makefile
new file mode 100644
index 0000000000000000000000000000000000000000..73d20c6744b465f4fb83afc3afe0e11ef31baf70
--- /dev/null
+++ b/quad/computation_graph/test/Makefile
@@ -0,0 +1,12 @@
+# QUAD_ROOT is obtained from environment
+SRC = $(QUAD_ROOT)/computation_graph/src/graph_blocks/*.c $(QUAD_ROOT)/computation_graph/src/computation_graph.c
+INC = $(QUAD_ROOT)/computation_graph/src
+BLOCKS_INC = $(QUAD_ROOT)/computation_graph/src/graph_blocks
+LIB = $(QUAD_ROOT)/lib/test
+
+test_computation_graph: test_computation_graph.c
+	gcc -o test_computation_graph -I. -I$(INC) -I$(BLOCKS_INC) -I$(LIB) $(LIB)/test.o test_computation_graph.c $(SRC) -lm
+
+.PHONY: clean
+clean:
+	rm test_computation_graph
diff --git a/quad/computation_graph/test/test_computation_graph b/quad/computation_graph/test/test_computation_graph
new file mode 100644
index 0000000000000000000000000000000000000000..e71c40e2e07e87e1250168a8615421f03265c47f
Binary files /dev/null and b/quad/computation_graph/test/test_computation_graph differ
diff --git a/quad/computation_graph/test/test_computation_graph.c b/quad/computation_graph/test/test_computation_graph.c
new file mode 100644
index 0000000000000000000000000000000000000000..8b11796af3a0042c5224cb65a37926185f17ee64
--- /dev/null
+++ b/quad/computation_graph/test/test_computation_graph.c
@@ -0,0 +1,157 @@
+#include "test.h"
+
+
+#include "computation_graph.h"
+#include "graph_blocks/node_add.h"
+#include "graph_blocks/node_mult.h"
+#include "graph_blocks/node_constant.h"
+#include "graph_blocks/node_gain.h"
+#include "graph_blocks/node_accumulator.h"
+
+#define GRAPH_TEST_EPS 0.00001
+
+static int nequal(double val1, double val2) {
+    if (fabs(val1 - val2) < GRAPH_TEST_EPS) {
+        return 0;
+    }
+    return -1;
+}
+
+int graph_test_one_add() {
+    struct computation_graph *graph = create_graph();
+    int block = graph_add_node_add(graph, "Add");
+    int cblock3 = graph_add_node_const(graph, "3");
+    graph_set_param_val(graph, cblock3, CONST_SET, 3);
+    int cblock4 = graph_add_node_const(graph, "4");
+    graph_set_param_val(graph, cblock4, CONST_SET, 4);
+    graph_set_source(graph, block, ADD_SUMMAND1, cblock3, CONST_VAL);
+    graph_set_source(graph, block, ADD_SUMMAND2, cblock4, CONST_VAL);
+    int to_compute_for[1] = {block};
+    graph_compute_nodes(graph, to_compute_for, 1);
+    double result = graph_get_output(graph, block, ADD_SUM);
+    return nequal(result, 7);
+}
+
+
+int graph_test_circular_runs() {
+    struct computation_graph *graph = create_graph();
+    int gain1 = graph_add_node_gain(graph, "gain1");
+    int gain2 = graph_add_node_gain(graph, "gain2");
+    graph_set_source(graph, gain2, GAIN_INPUT, gain1, GAIN_RESULT);
+    graph_set_source(graph, gain1, GAIN_INPUT, gain2, GAIN_RESULT);
+    int to_compute_for[1] = {gain2};
+    graph_compute_nodes(graph, to_compute_for, 1);
+    // If no infinite loop, then success. Value is undefined for circular graphs
+    return 0;
+}
+
+int graph_test_circular_resets() {
+    struct computation_graph *graph = create_graph();
+    int acum1 = graph_add_node_accum(graph, "accumulator1");
+    int acum2 = graph_add_node_accum(graph, "accumulator2");
+    graph_set_source(graph, acum2, ACCUM_IN, acum1, ACCUMULATED);
+    graph_set_source(graph, acum1, ACCUM_IN, acum2, ACCUMULATED);
+    return 0; // Passes if no infinite loop
+}
+
+// Tests the accumulator block, thereby testing reset and state changes
+int graph_test_accumulator() {
+    struct computation_graph *graph = create_graph();
+    int cblock = graph_add_node_const(graph, "const");
+    int acum_b = graph_add_node_accum(graph, "accumulator");
+    graph_set_source(graph, acum_b, ACCUM_IN, cblock, CONST_VAL);
+
+    int to_compute_for[1] = {acum_b};
+    graph_set_param_val(graph, cblock, CONST_SET, 3);
+    graph_compute_nodes(graph, to_compute_for, 1);
+    graph_set_param_val(graph, cblock, CONST_SET, 8);
+    graph_compute_nodes(graph, to_compute_for, 1);
+    graph_set_param_val(graph, cblock, CONST_SET, -2);
+    graph_compute_nodes(graph, to_compute_for, 1);
+
+    double result = graph_get_output(graph, acum_b, ACCUMULATED);
+    if (nequal(result, 9)) {
+        printf("graph_test_accumulator failed on step 1, equals %f\n", result);
+        return -1;
+    }
+
+    // Test reset on source set
+    int gain_b = graph_add_node_gain(graph, "Gain");
+    graph_set_param_val(graph, gain_b, GAIN_GAIN, 1);
+    graph_set_source(graph, gain_b, GAIN_INPUT, acum_b, ACCUMULATED);
+    to_compute_for[0] = gain_b;
+    graph_compute_nodes(graph, to_compute_for, 1);
+    result = graph_get_output(graph, gain_b, GAIN_RESULT);
+    if (nequal(result, -2)) {
+        printf("graph_test_accumulator failed on step 2\n");
+        return -2;
+    }
+    return 0;
+}
+
+// Tests that a block will only execute once per compute,
+// even if its output is connected to multiple inputs
+int graph_test_single_run() {
+    struct computation_graph *graph = create_graph();
+    int acum_b = graph_add_node_accum(graph, "accumulator");
+    int add_block = graph_add_node_add(graph, "Add");
+    int cblock = graph_add_node_const(graph, "const");
+    graph_set_param_val(graph, cblock, CONST_SET, 2);
+
+
+    graph_set_source(graph, acum_b, ACCUM_IN, cblock, CONST_VAL);
+    graph_set_source(graph, add_block, ADD_SUMMAND1, acum_b, ACCUMULATED);
+    graph_set_source(graph, add_block, ADD_SUMMAND2, acum_b, ACCUMULATED);
+
+    int to_compute_for[1] = {add_block};
+    graph_compute_nodes(graph, to_compute_for, 1);
+    double result = graph_get_output(graph, add_block, ADD_SUM);
+    return nequal(result, 4);
+}
+
+// Tests that upon connection of a second child, a block will not reset
+int graph_test_reset_rules() {
+    struct computation_graph *graph = create_graph();
+    int cblock = graph_add_node_const(graph, "5");
+    graph_set_param_val(graph, cblock, CONST_SET, 5);
+    int acum_b = graph_add_node_accum(graph, "accumulator");
+    int gain1 = graph_add_node_gain(graph, "gain1");
+    graph_set_param_val(graph, gain1, GAIN_GAIN, 1);
+
+    graph_set_source(graph, gain1, GAIN_INPUT, acum_b, ACCUMULATED);
+    graph_set_source(graph, acum_b, ACCUM_IN, cblock, CONST_VAL);
+    int to_compute_for[1] = {gain1};
+    graph_compute_nodes(graph, to_compute_for, 1);
+    // state of acum_b is now 5
+
+    int gain2 = graph_add_node_gain(graph, "gain2");
+    graph_set_param_val(graph, gain2, GAIN_GAIN, 1);
+    // Connect gain 2, and accumulator should not get reset
+    graph_set_source(graph, gain2, GAIN_INPUT, acum_b, ACCUMULATED);
+    to_compute_for[0] = gain2;
+    graph_compute_nodes(graph, to_compute_for, 1);
+    double result = graph_get_output(graph, gain2, GAIN_RESULT);
+    return nequal(result, 10);
+}
+
+int graph_test_self_loop() {
+    struct computation_graph *graph = create_graph();
+    int gain1 = graph_add_node_gain(graph, "gain1");
+    graph_set_source(graph, gain1, GAIN_INPUT, gain1, GAIN_RESULT);
+    int to_compute_for[1] = {gain1};
+    graph_compute_nodes(graph, to_compute_for, 1);
+    return 0;
+}
+
+
+int main() {
+    int success = 0;
+    test(graph_test_one_add, "Test adding 2 numbers");
+    test(graph_test_circular_runs, "Test computing cycles");
+    test(graph_test_circular_resets, "Test resetting cycles");
+    test(graph_test_accumulator, "Test accumulator (state)");
+    test(graph_test_single_run, "Test that blocks only get executed once");
+    test(graph_test_reset_rules, "Tests that already connected blocks don't get reset");
+    test(graph_test_self_loop, "Tests that a self-loop computation terminates");
+    test_summary();
+}
diff --git a/quad/test-ci.sh b/quad/test-ci.sh
index c35802b3984f5ba6dd4ba5984136340dec297e35..b7f71127b847e302fa5a341d8b7c76bcdbb89469 100644
--- a/quad/test-ci.sh
+++ b/quad/test-ci.sh
@@ -9,3 +9,7 @@ make || exit 1
 cd $QUAD_ROOT/sw/modular_quad_pid/test
 make || exit 1
 ./test_uart_buff || exit 1
+
+cd $QUAD_ROOT/sw/computation_graph/test
+make || exit 1
+./test_computation_graph || exit 1