diff --git a/tests/gy521_driver/test_gy521_driver.c b/tests/gy521_driver/test_gy521_driver.c
index c18413a..48a7fb5 100644
--- a/tests/gy521_driver/test_gy521_driver.c
+++ b/tests/gy521_driver/test_gy521_driver.c
@@ -1,3 +1,4 @@
+#include <stdio.h>
 #include <stdarg.h>
 #include <stddef.h>
 #include <stdint.h>
@@ -7,6 +8,7 @@
 #include "gy521_driver.h"
 
 #define WRITE_BIT   (1<<7)
+#define     REG_SIZE    32 
 
 typedef struct reg_addr{
     uint8_t dev_addr;
@@ -14,40 +16,46 @@ typedef struct reg_addr{
     uint8_t value;
 }reg_addr;
 
-reg_addr reg_addr_arr[16] = {{0x0, 0x0, 0x0}};
+reg_addr reg_addr_arr[REG_SIZE] = {{0x0, 0x0, 0x0}};
 uint8_t idx = 0;
 
 /* Fake Object for TWI_TX*/
 void fake_twi_tx(uint8_t slave_addr, uint8_t *data, uint8_t size)
 {
-    /*Save the slave address passed*/
-    reg_addr_arr[0].dev_addr = slave_addr;
 
-    /*Now write all the passed data*/
-    for(; idx < size; idx++) {
-        reg_addr_arr[idx].addr = *(data);
-        reg_addr_arr[idx].value = *(++data);
-    } 
+    /*read the device address.*/
+    reg_addr_arr[idx].dev_addr = slave_addr;
+
+    /*Save the register it's trying to access.*/
+    reg_addr_arr[idx].addr = *data;
+
+    /*Save the data it's sending to that address.*/
+    reg_addr_arr[idx].value = *(data + 1);
+
     /*Post increment the global index*/
     idx++;
+    if(idx > REG_SIZE){
+        idx = 0;
+    }
 }
 
 /* Fake Object for TWI_RX*/
 void fake_twi_rx(uint8_t slave_addr, uint8_t *data, uint8_t size)
 {
-    /*Save the slave address passed*/
-    reg_addr_arr[0].dev_addr = slave_addr;
+    /*Read the device address*/
+    reg_addr_arr[idx].dev_addr = slave_addr;
+
+    /*Put down the register address it's trying to read.*/ 
+    reg_addr_arr[idx].addr = *data;
     
-    /*Read the registers requested*/
-    for(uint8_t i = 0; i < size; i++) {
-        /*Wridxte the register read address*/
-        reg_addr_arr[i + idx].addr = *(data + i);
-        
-        /*Read the response value into the pased data ptr*/
-        *(data + i) = reg_addr_arr[i + idx].value;
-    }
+    /*Fill it with the data from the register*/
+    *data = reg_addr_arr[idx].value;
+
     /*Post increment the global index*/
     idx++;
+    if(idx > REG_SIZE){
+        idx = 0;
+    }
 }
 
 
@@ -56,6 +64,42 @@ void fake_twi_rx(uint8_t slave_addr, uint8_t *data, uint8_t size)
 void (*gy521_twi_tx)(uint8_t, uint8_t*, uint8_t) = &fake_twi_tx;
 void (*gy521_twi_rx)(uint8_t, uint8_t*, uint8_t) = &fake_twi_rx;
 
+/*
+ * ############################
+ * Helper functions
+ * ############################
+ */ 
+
+
+void print_reg_arr(void)
+{
+    printf("FAKE REGISTERS::\n");
+    for(uint8_t i = 0; i < REG_SIZE; i++){
+        print_error("%d:: val: %d, addr: %d, dev_addr: %d\n",
+                i, 
+                reg_addr_arr[i].value,
+                reg_addr_arr[i].addr,
+                reg_addr_arr[i].dev_addr);
+    }
+}
+
+void clear_twi(void)
+{
+    idx = 0;
+    for(int i = 0; i < REG_SIZE; i++){
+        reg_addr_arr[i].value = 0;
+        reg_addr_arr[i].addr = 0;
+        reg_addr_arr[i].dev_addr = 0;
+    }
+}
+
+
+/*
+ * ############################
+ * TESTS
+ * ############################
+ */ 
+
 /* Tests the donothing function */ 
 static void test_gy521_init(void **state) { 
     gy521_module *m = gy521_new();
@@ -136,7 +180,7 @@ static void test_gy521_update_gyro(void **sate)
     gy521_init(m, TWI_GY521_ADDR1);
     
     /*Setup the fake gyro values.*/
-    for(uint8_t i = 0; i < 6; i++) {
+    for(uint8_t i = 1; i < 7; i++) {
         reg_addr_arr[i].value = i * 8;
     }
   
@@ -175,39 +219,40 @@ static void test_gy521_self_test(void **sate)
     gy521_init(m, TWI_GY521_ADDR1);
     
     /*Zero the global index for the twi*/
-    idx = 0;
-    
+    clear_twi(); 
     
     /*Setup the full-scale range to +-250dps*/
     
     /*Load up the gyro regs with passing test values*/
-    for(uint8_t i = 0; i < 6; i++) {
+    /*Load up the selftest regs with passing test values.*/
+    /*Load up the gyro regs for disabled ST reads*/
+    for(uint8_t i = 1; i < 17; i++) {   /*(6regs * 2) + 3ST regs = 15*/
         reg_addr_arr[i].value = 2; 
     }
-    gy521_update_gyro(m);
 
 
-
-    /*Load up the testing registers 13-15 with the FT values*/     
-    /*FT: factory trim*/
-    idx = 0;
-    for(int i = 0; i < 3; i++){
-        reg_addr_arr[i].value = 2;  /*The values all have to be 4bit*/
-    }
-
     self_test_results result = gy521_self_test(m); 
     assert_false(result);   /*The value should be zero, aka zero faults*/
-    
+
     /*Check to make sure it can fail.*/
-    
-    /*Load up bad values*/
-    idx = 0;
-    for(int i = 0; i < 6; i++){
-        reg_addr_arr[i].value = 16;  /*The values all have to be 4bit*/
+    clear_twi();
+    for(uint8_t i = 1; i < 5; i++){
+        reg_addr_arr[i].value = 2;
     }
-    
+    for(int i = 4; i < 9; i++){
+        reg_addr_arr[i].value = 255;
+    }
+    for(int i = 9; i < 17; i++){
+        reg_addr_arr[i].value = 2;
+    }
+        
     result = gy521_self_test(m);
-    assert_true(result);   /*The value should be greater than one.*/
+
+
+    /*Print out the register values*/
+    print_reg_arr();
+
+    assert_true(result); 
 
     /*Free the struct*/
     gy521_free(m);