diff --git a/ci-build.sh b/ci-build.sh
index 1828210237358ecbb38b5c82a26536f112281781..5b1ebeb80b37ddb5a1bb4c0db8818ead562cd0dc 100644
--- a/ci-build.sh
+++ b/ci-build.sh
@@ -3,5 +3,5 @@
 set -e
 
 # Quad Libraries and Boot image
-(cd quad && make)
+(cd quad && make deep-clean && make)
 #(cd groundStation && make)
diff --git a/ci-test.sh b/ci-test.sh
index c79ad9703580d64f4db35e914c7d5c2eb7134fb6..2b8060ce46483f7f59862936c1f69ff30e6c68bc 100644
--- a/ci-test.sh
+++ b/ci-test.sh
@@ -4,4 +4,4 @@ set -e
 export PATH=/usr/local/bin:$PATH
 
 # Quad
-(cd quad && make test)
+(cd quad && make deep-clean && make && make test)
diff --git a/quad/scripts/tests/test_safety_checks.rb b/quad/scripts/tests/test_safety_checks.rb
index f4e6904955db2d2fd99a220c6c79257e4ed4956a..f2f35d30d22d4eaf2a6d8eb3cafbb0d6d244945f 100644
--- a/quad/scripts/tests/test_safety_checks.rb
+++ b/quad/scripts/tests/test_safety_checks.rb
@@ -32,6 +32,7 @@ I2C_MPU_ACCEL_Y = "virt-quad-fifos/i2c-mpu-accel-y"
 I2C_MPU_ACCEL_Z = "virt-quad-fifos/i2c-mpu-accel-z"
 
 require 'test/unit/assertions'
+require 'timeout'
 require 'thread'
 include Test::Unit::Assertions
 
@@ -73,97 +74,106 @@ def get_motor_averages
   average
 end
 
-def check_led(on)
+def check_led(is_on)
   led = read_fifo_num(LED)
-  assert_equal(led, on)
+  assert_equal(led, is_on)
 end
 
-script_dir = File.expand_path(File.dirname(__FILE__))
-bin_dir = script_dir + "/../../bin/"
-Dir.chdir(bin_dir)
+Timeout::timeout(30) {
 
-# Start virtual quad
-quad = Process.spawn("./virt-quad")
+  puts("Setting up...")
 
-sleep 1
+  script_dir = File.expand_path(File.dirname(__FILE__))
+  bin_dir = script_dir + "/../../bin/"
+  Dir.chdir(bin_dir)
 
-#################
-#  Begin Tests
-#################
+  sleep 1
+
+  # Start virtual quad
+  quad_pid = Process.spawn("./virt-quad")
+
+  sleep 5
+
+  #################
+  #  Begin Tests
+  #################
 
-begin
-  puts("beginning tests")
-  
-  # Set gravity
-  File.write(I2C_MPU_ACCEL_Z, -1 * GRAVITY)
+  begin
 
-  puts("Check that motors are off at startup")
-  check_motors_are_off
+    puts("Beginning tests...")
 
-  puts("Check that LED is off at startup")
-  check_led(0)
+    # Set gravity
+    File.write(I2C_MPU_ACCEL_Z, -1 * GRAVITY)
 
-  puts("Check that increasing the throttle does nothing to motors")
-  # (because gear is still off)
-  for i in (THROTTLE_MIN..THROTTLE_MAX).step(1000)
-    File.write(THROTTLE, i)
+    puts("Check that motors are off at startup")
     check_motors_are_off
-    sleep 0.005
-  end
 
-  puts("Check that flipping gear to 1 while throttle is high does nothing")
-  # (motors should still be off, LED should still be off)
-  File.write(GEAR, GEAR_ON)
-  sleep 0.015
-  check_motors_are_off
-  i = THROTTLE_MAX
-  while i > THROTTLE_MID
-    i -= 1000
-    File.write(THROTTLE, i)
+    puts("Check that LED is off at startup")
+    check_led(0)
+
+    puts("Check that increasing the throttle does nothing to motors")
+    # (because gear is still off)
+    for i in (THROTTLE_MIN..THROTTLE_MAX).step(1000)
+      File.write(THROTTLE, i)
+      check_motors_are_off
+      sleep 0.005
+    end
+
+    puts("Check that flipping gear to 1 while throttle is high does nothing")
+    # (motors should still be off, LED should still be off)
+    File.write(GEAR, GEAR_ON)
+    sleep 0.015
+    check_motors_are_off
+    i = THROTTLE_MAX
+    while i > THROTTLE_MID
+      i -= 1000
+      File.write(THROTTLE, i)
+      check_motors_are_off
+      check_led 0
+      sleep 0.005
+    end
+
+    # (swtich GEAR back to off and bring throttle off)
+    File.write(GEAR, GEAR_OFF)
+    File.write(THROTTLE, THROTTLE_MIN)
+
+    puts("Check that the LED turns on when gear is flipped on")
+    # (motors should still be off because our throttle is low)
+    File.write(GEAR, GEAR_ON)
+    sleep 0.1
+    check_led 1
+    check_motors_are_off
+
+    puts("Check that motors turn on")
+    File.write(THROTTLE, THROTTLE_MID)
+    averages = get_motor_averages
+    average = (averages[0] + averages[1] + averages[2] + averages[3])/4
+    puts averages, "(#{average})"
+    assert average.between?(THROTTLE_EIGHTH, MOTOR_MAX)
+
+    # Check that gear switch kills the motors
+    # (and that light goes off)
+    File.write(GEAR, GEAR_OFF)
+    sleep 0.1
     check_motors_are_off
     check_led 0
-    sleep 0.005
-  end
 
-  # (swtich GEAR back to off and bring throttle off)
-  File.write(GEAR, GEAR_OFF)
-  File.write(THROTTLE, THROTTLE_MIN)
-
-  puts("Check that the LED turns on when gear is flipped on")
-  # (motors should still be off because our throttle is low)
-  File.write(GEAR, GEAR_ON)
-  sleep 0.020
-  check_led 1
-  check_motors_are_off
-
-  puts("Check that motors turn on")
-  File.write(THROTTLE, THROTTLE_MID)
-  averages = get_motor_averages
-  average = (averages[0] + averages[1] + averages[2] + averages[3])/4
-  puts averages, "(#{average})"
-  assert average.between?(THROTTLE_EIGHTH, MOTOR_MAX)
-
-  # Check that gear switch kills the motors
-  # (and that light goes off)
-  File.write(GEAR, GEAR_OFF)
-  sleep 0.040
-  check_motors_are_off
-  check_led 0
-
-  # (Bring the RC throttle back down)
-  File.write(THROTTLE, THROTTLE_MIN)
-
-  # Check that we can resume flight
-  File.write(GEAR, GEAR_ON)
-  sleep 0.040
-  check_led 1
+    # (Bring the RC throttle back down)
+    File.write(THROTTLE, THROTTLE_MIN)
 
-  sleep 1
-  puts "All safety checks passed."
+    # Check that we can resume flight
+    File.write(GEAR, GEAR_ON)
+    sleep 0.1
+    check_led 1
 
-ensure
+    sleep 1
+    puts "All safety checks passed."
 
-  Process.kill(9, quad)
+  ensure
 
-end
+    Process.kill(9, quad_pid)
+    Process.wait(quad_pid)
+
+  end
+}
 
diff --git a/quad/scripts/tests/test_unix_uart.rb b/quad/scripts/tests/test_unix_uart.rb
index 6621744e33f2a789bf191b33a58586b769f533f4..6b5d1469326581787b1278c5c0ac9748e74f2195 100644
--- a/quad/scripts/tests/test_unix_uart.rb
+++ b/quad/scripts/tests/test_unix_uart.rb
@@ -15,54 +15,64 @@ UART_TX = "virt-quad-fifos/uart-tx"
 
 require 'test/unit/assertions'
 require 'thread'
+require 'timeout'
 include Test::Unit::Assertions
 
-script_dir = File.expand_path(File.dirname(__FILE__))
-bin_dir = script_dir + "/../../bin/"
-Dir.chdir(bin_dir)
+Timeout::timeout(30) {
 
-# Start virtual quad
-quad = Process.spawn("./virt-quad")
+  puts("Setting up...")
 
-sleep 1
+  script_dir = File.expand_path(File.dirname(__FILE__))
+  bin_dir = script_dir + "/../../bin/"
+  Dir.chdir(bin_dir)
 
-#################
-#  Begin Tests
-#################
+  sleep 1
 
-begin
+  # Start virtual quad
+  quad_pid = Process.spawn("./virt-quad")
 
-  # Flip gear on
-  File.write(GEAR, GEAR_ON)
-  sleep 0.015
+  sleep 5
 
-  for j in 1..10
-    # Send a debug command
-    File.write(UART_RX, [0xBE, 1, 0, 0, 0, 0, 0, 0xBF].pack("CCCCCCCC"))
+  #################
+  #  Begin Tests
+  #################
 
-    fifo = File.open(UART_TX)
-    msg = []
-    for i in 1..7
-      sleep 0.010
-      msg.push(fifo.read(1))
-    end
-    length = msg[5..7].join().unpack("S")[0]
-    msg = []
-    for i in 1..length
-      sleep 0.010
-      msg.push(fifo.read(1))
-    end
-    fifo.close
+  begin
 
-    puts msg.join()
-    assert_equal(msg.join().force_encoding("UTF-8"), "Packets received: #{j}")
-  end
+    puts("Beginning tests...")
+
+    # Flip gear on
+    File.write(GEAR, GEAR_ON)
+    sleep 0.015
 
-  puts "Basic UART test passed."
+    for j in 1..10
+      # Send a debug command
+      File.write(UART_RX, [0xBE, 1, 0, 0, 0, 0, 0, 0xBF].pack("CCCCCCCC"))
 
-ensure
+      fifo = File.open(UART_TX)
+      msg = []
+      for i in 1..7
+        sleep 0.010
+        msg.push(fifo.read(1))
+      end
+      length = msg[5..7].join().unpack("S")[0]
+      msg = []
+      for i in 1..length
+        sleep 0.010
+        msg.push(fifo.read(1))
+      end
+      fifo.close
 
-  Process.kill(9, quad)
+      puts msg.join()
+      assert_equal(msg.join().force_encoding("UTF-8"), "Packets received: #{j}")
+    end
+
+    puts "Basic UART test passed."
 
-end
+  ensure
 
+    Process.kill(9, quad_pid)
+    Process.wait(quad_pid)
+
+  end
+}
diff --git a/quad/src/computation_graph/computation_graph.c b/quad/src/computation_graph/computation_graph.c
index 8720fcb9851edaea00864533d852dfb7653eee95..f20a0d0f50f4824fd41baab7f2adb80a3a8d1960 100644
--- a/quad/src/computation_graph/computation_graph.c
+++ b/quad/src/computation_graph/computation_graph.c
@@ -11,9 +11,9 @@ static double exec_input_vals[GRAPH_MAX_INPUTS];
 
 // Macro functions for setting and clearing single bits in int array
 // From http://www.mathcs.emory.edu/~cheung/Courses/255/Syllabus/1-C-intro/bit-array.html
-#define setBit(A,k)     ( A[(k / sizeof(int))] |=  (1 << (k % sizeof(int))) )
-#define clearBit(A,k)   ( A[(k / sizeof(int))] &= ~(1 << (k % sizeof(int))) )
-#define testBit(A,k)    ( A[(k / sizeof(int))] &   (1 << (k % sizeof(int))) )
+#define setBit(A,k)     ( A[(k / (8*sizeof(&A)))] |=  (1 << (k % (8*sizeof(&A)))) )
+#define clearBit(A,k)   ( A[(k / (8*sizeof(&A)))] &= ~(1 << (k % (8*sizeof(&A)))) )
+#define testBit(A,k)    ( A[(k / (8*sizeof(&A)))] &   (1 << (k % (8*sizeof(&A)))) )
 
 struct computation_graph *create_graph() {
     struct computation_graph *the_graph = malloc(sizeof(struct computation_graph));
@@ -74,7 +74,8 @@ int graph_set_source(struct computation_graph *graph,
     }
     struct graph_node *dest_node = &graph->nodes[dest_node_id];
     struct graph_node *src_node = &graph->nodes[src_node_id];
-    if (dest_input >= dest_node->type->n_inputs || src_output >= src_node->type->n_outputs) {
+    if (dest_input >= dest_node->type->n_inputs || src_output >= src_node->type->n_outputs ||
+        dest_input < 0 || src_output < 0) {
         return -1;
     }
 
@@ -120,16 +121,16 @@ int graph_add_node_id(struct computation_graph *graph,
         if (!node_arr) { return -1; }
         // Number of integers needed to hold new_size bits
         size_t new_exist_size = ceil((float)new_size / (8 * sizeof(int))); // ceil(new_size / (bits per int))
-        int* exist_arr = realloc(graph->node_existence, sizeof(int) * new_exist_size);
-        if (!exist_arr) {return -1;}
         // Set the newly allocated memory to 0
         size_t old_exist_size = ceil((float)old_size / (8 * sizeof(int)));
         if (old_exist_size != new_exist_size) {
+            int* exist_arr = realloc(graph->node_existence, sizeof(int) * new_exist_size);
+            if (!exist_arr) {return -1;}
             memset(exist_arr + old_exist_size, 0, (new_exist_size - old_exist_size) * sizeof(int));
+            graph->node_existence = exist_arr;
         }
         graph->size = new_size;
         graph->nodes = node_arr;
-        graph->node_existence = exist_arr;
     }
     struct graph_node *new_node = &graph->nodes[id];
     new_node->name = strdup(name);
@@ -182,14 +183,20 @@ double graph_get_output(const struct computation_graph *graph, int node_id, int
     return graph->nodes[node_id].output_values[output_id];
 }
 
+/*
+ * Assumptions: The node passed in is a valid ID (should be checked before passing)
+ * and all node sources are either valid node-output pairs, or the source node ID == -1
+ * These constraints should be satisfied by using the graph_set_source function, so long as
+ * a valid node ID is passed in to the first call of this function
+*/
 void graph_compute_node_rec(struct computation_graph *graph, int node_id, int depth) {
     if (depth >= GRAPH_MAX_DEPTH) {
-        assert(1 == 0); // TODO :Xil_Assert false
-        return;
-    }
-    if (!graph_node_exists(graph, node_id)) {
+        assert(1 == 0);
         return;
     }
+    // if (!graph_node_exists(graph, node_id)) {
+    //     return;
+    // }
     struct graph_node *node = &graph->nodes[node_id];
     if (node->processed_state != UNPROCESSED) {
         return;
@@ -226,9 +233,8 @@ void graph_compute_node_rec(struct computation_graph *graph, int node_id, int de
 void graph_compute_nodes(struct computation_graph *graph, int* node_ids, int n_nodes) {
     int i;
     for (i = 0; i < graph->size; i++) {
-        if (graph_node_exists(graph, i)) {
-            graph->nodes[i].processed_state = UNPROCESSED;
-        }
+        // Note: Do not access malloc'd members in here without first checking if node is valid
+        graph->nodes[i].processed_state = UNPROCESSED;
     }
     for (i = 0; i < n_nodes; i++) {
     	int node_id = node_ids[i];
@@ -238,11 +244,10 @@ void graph_compute_nodes(struct computation_graph *graph, int* node_ids, int n_n
     }
     // Clear all the updated flags for nodes that were actually executed
     for (i = 0; i < graph->size; i++) {
-        if (graph_node_exists(graph, i)) {
-            struct graph_node* node = &graph->nodes[i];
-            if (node->processed_state == PROCESSED) {
-                node->updated = 0;
-            }
+        // Note: Do not access malloc'd members in here without first checking if node is valid
+        struct graph_node* node = &graph->nodes[i];
+        if (node->processed_state == PROCESSED) {
+            node->updated = 0;
         }
     }
 }
diff --git a/quad/src/virt_quad/hw_impl_unix_i2c.c b/quad/src/virt_quad/hw_impl_unix_i2c.c
index 9de9815c2f7a941b0b553df525a3436f069ae0c4..c32e0c4ea56546a7699ee32e0cfc1170566f167f 100644
--- a/quad/src/virt_quad/hw_impl_unix_i2c.c
+++ b/quad/src/virt_quad/hw_impl_unix_i2c.c
@@ -20,14 +20,8 @@ static short last_dev;
 static short last_reg;
 static short last_val;
 
-static int zero = 0;
-static int one = 1;
-static int two = 2;
-static int three = 3;
-static int four = 4;
-static int five = 5;
-
-static pthread_t worker;
+static int nums[] = {0, 1, 2, 3, 4, 5};
+static pthread_t workers[6];
 
 int unix_i2c_reset(struct I2CDriver *self) {
   input_names[0] = "i2c-mpu-accel-x";
@@ -40,12 +34,10 @@ int unix_i2c_reset(struct I2CDriver *self) {
   mkdir(VIRT_QUAD_FIFOS_DIR, 0777);
 
   // Start up worker thread whose job is to update the caches
-  pthread_create(&worker, 0, update_i2c_input_cache, &zero);
-  pthread_create(&worker, 0, update_i2c_input_cache, &one);
-  pthread_create(&worker, 0, update_i2c_input_cache, &two);
-  pthread_create(&worker, 0, update_i2c_input_cache, &three);
-  pthread_create(&worker, 0, update_i2c_input_cache, &four);
-  pthread_create(&worker, 0, update_i2c_input_cache, &five);
+  int i;
+  for (i = 0; i < 6; i += 1) {
+    pthread_create(&workers[i], 0, update_i2c_input_cache, &nums[i]);
+  }
 
   cache[0].s = 0;
   cache[1].s = 0;
diff --git a/quad/src/virt_quad/hw_impl_unix_pwm_input.c b/quad/src/virt_quad/hw_impl_unix_pwm_input.c
index b487461095d1a900c16ea2f515454c650d197f3f..49bf19385f79053a50d58a47965bb81a0f7da6cf 100644
--- a/quad/src/virt_quad/hw_impl_unix_pwm_input.c
+++ b/quad/src/virt_quad/hw_impl_unix_pwm_input.c
@@ -10,7 +10,8 @@ void * update_input_cache();
 static char *input_names[6];
 static int fifos[6];
 static unsigned long cache[6];
-pthread_t worker;
+static pthread_t workers[6];
+static int nums[] = {0, 1, 2, 3, 4, 5};
 
 int unix_pwm_input_reset(struct PWMInputDriver *self) {
   input_names[0] = "pwm-input-throttle";
@@ -25,7 +26,8 @@ int unix_pwm_input_reset(struct PWMInputDriver *self) {
   // Start up worker thread whose job is to update the caches
   int i;
   for (i = 0; i < 6; i += 1) {
-    pthread_create(&worker, 0, update_input_cache, &i);
+    pthread_create(&workers[i], 0, update_input_cache, &nums[i]);
+    usleep(1000);
   }
 
   cache[0] = THROTTLE_MIN;