diff --git a/.gitmodules b/.gitmodules
index 1d7f28ad9b1e45d495b9440664eac40bad1ac6cf..6821767e986fef35ecf107d7b79b6ae27c0834bb 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -1,3 +1,4 @@
 [submodule "groundStation/src/vrpn"]
 	path = groundStation/src/vrpn
 	url = https://github.com/vrpn/vrpn
+	ignore = dirty
diff --git a/ci-build.sh b/ci-build.sh
index f2ee59eb3b1ecf1ff64936b8e32e215800177266..5b7ac989fe13ff8797d142124bda419aea73d0d2 100644
--- a/ci-build.sh
+++ b/ci-build.sh
@@ -3,4 +3,5 @@
 set -e
 
 # Quad Libraries and Boot image
-cd quad && make boot
+(cd quad && make)
+(cd groundStation && make) 
diff --git a/common/common.txt b/common/common.txt
deleted file mode 100644
index 3a20094073732be5f5bed0035644f2193a85cdbc..0000000000000000000000000000000000000000
--- a/common/common.txt
+++ /dev/null
@@ -1,3 +0,0 @@
-The common files are:
- - setcontrol.h
- - setcontrol.c
diff --git a/common/examples_c b/common/examples_c
deleted file mode 100644
index 036ddeb3c0443a88d42568cbcce6250f7e20ca83..0000000000000000000000000000000000000000
--- a/common/examples_c
+++ /dev/null
@@ -1,77 +0,0 @@
-#include "packet.h"
-#include "setcontrol.h"
-#include "commands.h"
-
-#include <sys/types.h>
-
-
-/* This example is close to how the ground station will handle things. It
- * shouldn't be too hard to adapt it to work on the quad side.
- */
-
-static int next_msg_id = 0;
-
-int SendSetControl()
-{
-	struct metadata m;
-	struct controller_message cm;
-
-	cm.id = ROLL_ID;        /* Roll controller */
-	cm.value_id = KI_ID;    /* I coefficient */
-	cm.value = 42.00f;      /* An appropriate value */
-
-	uint8_t data[64];
-	m.msg_id = next_msg_id++;
-	/* Fills in the rest of the metadata automatically */
-	if (EncodeSetcontrol(&m, data, 64, &cm) < 0) {
-		return -1;
-	}
-
-	uint8_t packet[128];
-	ssize_t length;
-	if ((length = EncodePacket(packet, 128, &m, data)) < 0) {
-		return -1;
-	}
-
-	/* Now you can send the packet
-	 * sendThePacket(packet, length);
-	 */
-
-	return 0;
-}
-
-
-/* Process incoming packets */
-int receive_packet(uint8_t * packet, size_t length)
-{
-	struct metadata m;
-	uint8_t data[128];
-
-	/* Will fail if checksum is bad, etc */
-	if (DecodePacket(&m, data, 128, packet, length) < 0) {
-		return -1;
-	}
-
-	/* Let's pretend that the callbacks have signature
-	 * void cb(
-	 *     const struct metadata * m,
-	 *     const uint8_t * data);
-	 *
-	 * Call the callback.
-	 */
-	(*MessageTypes[m.msg_type].functionPtr)(&m, data);
-
-	return 0;
-}
-
-/* An example of a setcontrol callback. Arguments
- * aren't important, as long as we have access
- * to the packet bytes and the length
- */
-void cb_setcontrol(const struct metadata *m, const uint8_t *data)
-{
-	struct controller_message cm;
-	DecodeSetcontrol(&cm, m, data);
-
-	/* cm now has populated controller ID, value ID, and value */
-}
diff --git a/controls/DataAnalysisTool/Tool/DataAnalysis.m b/controls/DataAnalysisTool/Tool/DataAnalysis.m
index 34834d0460e095c21808b0cc26c5d084546a9319..5cd77413d324f07efd498f48265e068b096ef1b3 100644
--- a/controls/DataAnalysisTool/Tool/DataAnalysis.m
+++ b/controls/DataAnalysisTool/Tool/DataAnalysis.m
@@ -8,14 +8,14 @@
 % window.
 %
 %fname = 'sampleLogFileWithMarker.txt';  
-fname = 'testies_v2.txt';  
+fname = '';  
 
 % PLOTTING SWITCHES - set them to 0 or 1 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 %
 plot            = 1;    % to choose plotting
 separatePlot    = 1;    % to generate separatePlots
 multiPlot       = 1;    % to generate multiPlot
-subPlot         = 0;    % to generate subPlots
+subPlot         = 1;    % to generate subPlots
 clearFigs       = 0;    % to close all the plots (needed only by the GUI)
 
 % DATA TO PLOT %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -25,27 +25,28 @@ clearFigs       = 0;    % to close all the plots (needed only by the GUI)
 % seperatePlots. If this is empty but "plot" switch is 1, all the columns
 % will be plotted using seperatePlots.
 %
-separateData = {'PID_roll_vel'};
+separateData = {'X pos PID_Correction', 'Altitude PID_Correction'};
                                                 
 % MULTIDATA
 % write names of the data headers that you want to plot using multiPlots.
 % If this is empty but "plot" switch is 1, the data headers in separateData
 % will be plotted using multiPlots.
 %
-multiData = {}; 
+multiData = {'X pos PID_Correction', 'Altitude PID_Correction'}; 
 
 % SUBDATA
 % write names of the data headers that you want to plot using subPlots.
 % If this is empty but "plot" switch is 1, the data headers in multiData
 % will be plotted using subPlots.
 %
-subData = {};
+subData = {'X pos PID_Correction', 'Altitude PID_Correction'};
+
 
 % COLOR FOR PLOTTING %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 %
 color       = 'r';      % one character for color of the plotting line
 marker      = '';       % one character for the marker of the plotting line
-style       = ':';      % one character for the style of the plotting line
+style       = '-';      % one character for the style of the plotting line
 backgnd     = [1 1 1];  % rgb array for background color of the plot
 
 
@@ -94,10 +95,9 @@ expData = parse_log(params.file.pathName, params);
 % plot the data accoriding to the plotting parameters set
 %
 plot_data(expData, params.plotting);
-
 %% creating the main structure to be stored in the workspace %%%%%%%%%%%%%
 %
-main.params     = params;
-main.expData    = expData;
-clearvars -except main;
-save main main;
\ No newline at end of file
+% main.params     = params;
+% main.expData    = expData;
+% clearvars -except main;
+% save main main;
\ No newline at end of file
diff --git a/controls/DataAnalysisTool/Tool/main.mat b/controls/DataAnalysisTool/Tool/main.mat
deleted file mode 100644
index 2385f3363d899a77037979f39fda9055a9a94454..0000000000000000000000000000000000000000
Binary files a/controls/DataAnalysisTool/Tool/main.mat and /dev/null differ
diff --git a/controls/DataAnalysisTool/Tool/parse_log.m b/controls/DataAnalysisTool/Tool/parse_log.m
index f265c9c849b881ba8452362518bed7b4106f994d..acdf40e77d7f1294c58d8c2a26c39a741cf382a9 100644
--- a/controls/DataAnalysisTool/Tool/parse_log.m
+++ b/controls/DataAnalysisTool/Tool/parse_log.m
@@ -43,6 +43,7 @@ identifier = string(1);
 if (regexp(identifier,'%'))
     foundHeaders = 1;
     % this is a line of headers; extract headers:
+    string = strrep(string,' ', '_');
     headers = strsplit(string);
     headers{1} = strrep(headers{1},'%', '');
     numOfHeaders = length(headers);
@@ -126,17 +127,20 @@ if exist('params', 'var')
                         end
                     else
                         for i = 1:length(params.plotting.separateData)
+                            params.plotting.separateData{i} = strrep(params.plotting.separateData{i},' ', '_');
                             eval(['loggedData.' params.plotting.separateData{i} '.params.plot = 1;']);
                         end
                     end
                     
                 else
                     for i = 1:length(params.plotting.multiData)
+                        params.plotting.multiData{i} = strrep(params.plotting.multiData{i},' ', '_');
                         eval(['loggedData.' params.plotting.multiData{i} '.params.plot = 1;']);
                     end
                 end
             else
                 for i = 1:length(params.plotting.subData)
+                    params.plotting.subData{i} = strrep(params.plotting.subData{i},' ', '_');
                     eval(['loggedData.' params.plotting.subData{i} '.params.plot = 1;']);
                 end
             end
@@ -150,11 +154,13 @@ if exist('params', 'var')
                     end
                 else
                     for i = 1:length(params.plotting.separateData)
+                        params.plotting.separateData{i} = strrep(params.plotting.separateData{i},' ', '_');
                         eval(['loggedData.' params.plotting.separateData{i} '.params.plot = 1;']);
                     end
                 end
             else
                 for i = 1:length(params.plotting.multiData)
+                    params.plotting.multiData{i} = strrep(params.plotting.multiData{i},' ', '_');
                     eval(['loggedData.' params.plotting.multiData{i} '.params.plot = 1;']);
                 end
             end
@@ -167,6 +173,7 @@ if exist('params', 'var')
                 end
             else
                 for i = 1:length(params.plotting.separateData)
+                    params.plotting.separateData{i} = strrep(params.plotting.separateData{i},' ', '_');
                     eval(['loggedData.' params.plotting.separateData{i} '.params.plot = 1;']);
                 end
             end
diff --git a/controls/DataAnalysisTool/Tool/plot_data.m b/controls/DataAnalysisTool/Tool/plot_data.m
index ea7fd3410c23971dde2515f85c6fb0e0821de0c7..31119a99856a6d0732138d7d2b6b8a1b3943dd8c 100644
--- a/controls/DataAnalysisTool/Tool/plot_data.m
+++ b/controls/DataAnalysisTool/Tool/plot_data.m
@@ -6,6 +6,8 @@ function plot_data(expData, plotParams)
 % check plotting switch
 if (~plotParams.plot) 
     disp('No plotting');
+elseif(~plotParams.multiPlot && ~plotParams.subPlot && ~plotParams.separatePlot)
+    disp('No plotting, select a plotting style');
 else
     %% check separate plotting switch
     if(plotParams.separatePlot)
@@ -74,7 +76,7 @@ else
         if(isempty(plotParams.subData)) % if none mentioned, use headers mentioned for multi plotting
             if(isempty(plotParams.multiData)) % if no headers mentioned for multi plotting, use headers mentioned for separate plotting
                 if(isempty(plotParams.separateData)) % if no headers mentioned for separate plotting, use all of the headers
-                    headers = fieldNames(expData);
+                    headers = fieldnames(expData);
                 else
                     headers = plotParams.separateData;
                 end
diff --git a/controls/DataAnalysisTool/Tool/plot_multi.m b/controls/DataAnalysisTool/Tool/plot_multi.m
index d63c9df36fddf81cefcee997e480e8ade2c3a38f..15bdcab7a569ffbd1849803bab754c20f73e9dc7 100644
--- a/controls/DataAnalysisTool/Tool/plot_multi.m
+++ b/controls/DataAnalysisTool/Tool/plot_multi.m
@@ -13,6 +13,7 @@ function plot_multi(expData, useMarker, varargin)
 numOfHeaders = 0;
 headers = {};
 for i = 1:length(varargin)
+    varargin{i} = strrep(varargin{i},' ', '_');
     if(~isPlotCharString(varargin{i}))
         numOfHeaders = numOfHeaders + 1;
         headers{numOfHeaders} = varargin{i};
@@ -144,13 +145,15 @@ for i = 1:numOfHeaders
     end
 end
 
-ylabel(yAxisLabel);
+ylabel(yAxisLabel,'Interpreter', 'none');
 
 % constructing statement for inserting the legend
-legendString = ['legend('''];
+legendString = ['legend('];
 for i = 1:numOfHeaders
     if(i == numOfHeaders)
-        legendString = strcat(legendString,headers{i},''',''Location'',''NorthWest'');');
+        legendString = strcat(legendString,headers{i},'''}',',''Interpreter'',''none'',''Location'',''NorthWest'');');
+    elseif(i == 1)
+        legendString = strcat(legendString,'{''',headers{i},''',''');
     else
         legendString = strcat(legendString,headers{i},''',''');
     end
diff --git a/controls/DataAnalysisTool/Tool/plot_separate.m b/controls/DataAnalysisTool/Tool/plot_separate.m
index 21258d6dacefad42184b8a649f738b7d9a5ee11a..c429b4ff17362bc559876c67cfb79bf3e2e27ab0 100644
--- a/controls/DataAnalysisTool/Tool/plot_separate.m
+++ b/controls/DataAnalysisTool/Tool/plot_separate.m
@@ -25,6 +25,7 @@ end
 % bulding the plot statement that will be executed
 for i = 1:length(varargin)
     
+    varargin{i} = strrep(varargin{i},' ', '_');
     dataHeader = varargin{i};
     
     % continue to next iteration if current argument is a plotting character string
@@ -49,19 +50,19 @@ for i = 1:length(varargin)
         plotCharString = buildPlotCharString(eval(['expData.' dataHeader '.params']));
         plotString = strcat(plotString,',''', plotCharString, ''');');
     end
-    
+
     % plotting data and making it look good
     figure;
     whitebg(gcf,eval(['expData.' dataHeader '.params.backgnd']));   % setting background
     eval(plotString);   % plotting
-    title(dataHeader);  % setting title
+    title(dataHeader, 'Interpreter', 'none');  % setting title
     xlabel(['Time (' time.unit ')']);   % setting the x-axis label
     if(eval(['isempty(expData.' dataHeader '.unit)']))
         yAxisLabel = dataHeader;
     else
         yAxisLabel = [dataHeader ' (' eval(['expData.' dataHeader '.unit']) ')'];
     end
-    ylabel(yAxisLabel); % setting y-axis label
+    ylabel(yAxisLabel,'Interpreter', 'none'); % setting y-axis label
     xlim([0, time.data(end)]); % setting x-axis limits
     grid ON;    % setting grid lines on the graph
     % adding markers
diff --git a/controls/DataAnalysisTool/Tool/plot_sub.m b/controls/DataAnalysisTool/Tool/plot_sub.m
index f91b59e2ff472dc38a39ed3575176a7a6c291f00..e088eb2c4724994f7369281e749a62710c894b76 100644
--- a/controls/DataAnalysisTool/Tool/plot_sub.m
+++ b/controls/DataAnalysisTool/Tool/plot_sub.m
@@ -41,6 +41,8 @@ for i = 1:length(varargin)
         continue;
     end
     
+    varargin{i} = strrep(varargin{i},' ', '_');
+    
     % keeping track of the number of headers
     currHeaderIndex = currHeaderIndex + 1;
     
@@ -75,14 +77,14 @@ for i = 1:length(varargin)
     
     %% plotting data and making it look pretty
     eval(plotString);
-    title(varargin{i});
+    title(varargin{i},'Interpreter','none');
     xlabel(['Time (' time.unit ')']);
     if(eval(['isempty(expData.' varargin{i} '.unit)']))
         yAxisLabel = varargin{i};
     else
         yAxisLabel = [varargin{i} ' (' eval(['expData.' varargin{i} '.unit']) ')'];
     end
-    ylabel(yAxisLabel);
+    ylabel(yAxisLabel,'Interpreter','none');
     xlim([0,time.data(end)]);
     grid ON;
     set(gca,'Color',eval(['expData.' varargin{i} '.params.backgnd']));
diff --git a/controls/model/.gitignore b/controls/model/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..fdd0385d7c2df2b890075dc24d1f80bb1e671391
--- /dev/null
+++ b/controls/model/.gitignore
@@ -0,0 +1,3 @@
+slprj/
+test_model_grt_rtw/
+test_model_sfun.mexw64
\ No newline at end of file
diff --git a/controls/model/c_controller.c b/controls/model/c_controller.c
index 6bcee0997d9c72199d6983b738bac881478fdbe6..aff3ffbc6aad17c2751048c88c508d7834d74399 100644
--- a/controls/model/c_controller.c
+++ b/controls/model/c_controller.c
@@ -1,6 +1,6 @@
 #include "c_controller.h"
-#include "quad_files/control_algorithm.c"
-#include "quad_files/computation_graph.c"
+#include "control_algorithm.c"
+#include "computation_graph.c"
 double c_controller(int vrpn_id, double vrpn_ts, double set_x, double set_y, double set_z, double set_yaw,
         double cur_x, double cur_y, double cur_z,
         double cur_phi, double cur_theta, double cur_psi,
diff --git a/controls/model/c_controller.h b/controls/model/c_controller.h
index e53b1a508409d70211a99fe6ba6de76e092ae7a5..4cf3a88f751129dd1777898ae599358964d012e5 100644
--- a/controls/model/c_controller.h
+++ b/controls/model/c_controller.h
@@ -1,12 +1,12 @@
 #ifndef C_CONTROLLER_H
 #define C_CONTROLLER_H
-#include "quad_files/computation_graph.h"
-#include "quad_files/graph_blocks/node_pid.h"
-#include "quad_files/graph_blocks/node_bounds.h"
-#include "quad_files/graph_blocks/node_constant.h"
-#include "quad_files/graph_blocks/node_mixer.h"
-#include "quad_files/PID.h"
-#include "quad_files/control_algorithm.h"
+#include "computation_graph.h"
+#include "node_pid.h"
+#include "node_bounds.h"
+#include "node_constant.h"
+#include "node_mixer.h"
+#include "PID.h"
+#include "control_algorithm.h"
 
 double c_controller(int vrpn_id, double vrpn_ts, double set_x, double set_y, double set_z, double set_yaw,
         double cur_x, double cur_y, double cur_z,
diff --git a/controls/model/loggingAnalysis/logAnalysis.m b/controls/model/loggingAnalysis/logAnalysis.m
index ae48f90734c220bf920b8367ca5fca88411d84c7..e1ba18ae230e9a3641771d0879861cb59a09f734 100644
--- a/controls/model/loggingAnalysis/logAnalysis.m
+++ b/controls/model/loggingAnalysis/logAnalysis.m
@@ -32,10 +32,10 @@ yaw_command_model_data = yaw_command_model.signals.values(indices_40ms);
 yaw_value_model_data = yaw_value_model.signals.values(indices_40ms);
 
 % Pull duty cycle commands from model
-PWM0_model = motorCommands.signals.values(indices_40ms, 1);
-PWM1_model = motorCommands.signals.values(indices_40ms, 2);
-PWM2_model = motorCommands.signals.values(indices_40ms, 3);
-PWM3_model = motorCommands.signals.values(indices_40ms, 4);
+PWM0_model = motorCommands.signals.values(indices_5ms, 1);
+PWM1_model = motorCommands.signals.values(indices_5ms, 2);
+PWM2_model = motorCommands.signals.values(indices_5ms, 3);
+PWM3_model = motorCommands.signals.values(indices_5ms, 4);
 
 %% Plot x control structure
 
@@ -165,7 +165,7 @@ legend('Log', 'Model', 'location', 'northwest');
 %% Plot PWM Commands
 figure(5); subplot(2, 2, 1);
 stairs(time, PWM0,'.-'); hold on; grid minor;
-stairs(time_model_40ms, PWM0_model, '.-'); hold off;
+stairs(time_model_5ms, PWM0_model, '.-'); hold off;
 title('PWM0 Value');
 xlabel('Time (s)');
 ylabel('PWM0 Command');
@@ -173,7 +173,7 @@ legend('Log', 'Model', 'location', 'northwest');
 
 subplot(2, 2, 2);
 stairs(time, PWM1,'.-'); hold on; grid minor;
-stairs(time_model_40ms, PWM1_model, '.-'); hold off;
+stairs(time_model_5ms, PWM1_model, '.-'); hold off;
 title('PWM1 Value');
 xlabel('Time (s)');
 ylabel('PWM1 Command');
@@ -181,7 +181,7 @@ legend('Log', 'Model', 'location', 'northwest');
 
 subplot(2, 2, 3);
 stairs(time, PWM2,'.-'); hold on; grid minor;
-stairs(time_model_40ms, PWM2_model, '.-'); hold off;
+stairs(time_model_5ms, PWM2_model, '.-'); hold off;
 title('PWM2 Value');
 xlabel('Time (s)');
 ylabel('PWM2 Command');
@@ -189,7 +189,7 @@ legend('Log', 'Model', 'location', 'northwest');
 
 subplot(2, 2, 4);
 stairs(time, PWM3,'.-'); hold on; grid minor;
-stairs(time_model_40ms, PWM3_model, '.-'); hold off;
+stairs(time_model_5ms, PWM3_model, '.-'); hold off;
 title('PWM3 Value');
 xlabel('Time (s)');
 ylabel('PWM3 Command');
diff --git a/controls/model/modelParameters.m b/controls/model/modelParameters.m
index 97115a7c498158b6e5d7765cb3de075772fee193..bda6e93514a28d95838ee3a62e6ded80d694facf 100644
--- a/controls/model/modelParameters.m
+++ b/controls/model/modelParameters.m
@@ -1,4 +1,4 @@
-temp = 0;
+temp = 1;
 % Log Analysis Toggle    
     logAnalysisToggle = 1;          % 1 for log analysis, 0 for normal operation
 
@@ -181,7 +181,7 @@ elseif logAnalysisToggle == 1 && temp == 1
     % window.
     %
     %fname = 'sampleLogFile.txt';  
-    fname = 'sampleLogFile.txt';
+    fname = '';
     fpath = '';
     
     if(isempty(fname))
@@ -208,8 +208,8 @@ elseif logAnalysisToggle == 1 && temp == 1
     y_error = timeseries(y_setpoint - y_position, time);
 
     % Determine z position error
-    z_setpoint = dataStruct.Z_Setpoint_Constant.data;
-    z_position = dataStruct.VRPN_Z_Constant.data;
+    z_setpoint = dataStruct.Alt_Setpoint_Constant.data;
+    z_position = dataStruct.VRPN_Alt_Constant.data;
     z_error = timeseries(z_setpoint - z_position, time);
 
     % Determine pitch error
@@ -243,10 +243,10 @@ elseif logAnalysisToggle == 1 && temp == 1
     yawrate_error = timeseries(yawrate_setpoint - yawrate_value, time);
 
     % Pull motor commands from log
-    x_command = dataStruct.Pitch_Rate_PID_Correction;
-    y_command = dataStruct.Roll_Rate_PID_Correction;
-    z_command = dataStruct.Altitude_PID_Correction;
-    yaw_command = dataStruct.Yaw_Rate_PID_Correction;
+    x_command = dataStruct.Pitch_Rate_PID_Correction.data;
+    y_command = dataStruct.Roll_Rate_PID_Correction.data;
+    z_command = dataStruct.Altitude_PID_Correction.data;
+    yaw_command = dataStruct.Yaw_Rate_PID_Correction.data;
     
     % Determine signal mix PWM values
     PWM0 = dataStruct.Signal_Mixer_PWM_0.data;
diff --git a/controls/model/parse_log_model.m b/controls/model/parse_log_model.m
index 9fba92fa6d0d3d670370b06a035fd9ee802698d2..59aef835bd0d5077e9619b7c02880a8390d95643 100644
--- a/controls/model/parse_log_model.m
+++ b/controls/model/parse_log_model.m
@@ -43,6 +43,7 @@ identifier = string(1);
 if (regexp(identifier,'%'))
     foundHeaders = 1;
     % this is a line of headers; extract headers:
+    string = strrep(string,' ', '_');
     headers = strsplit(string);
     headers{1} = strrep(headers{1},'%', '');
     numOfHeaders = length(headers);
@@ -106,6 +107,5 @@ for i = 1:numOfHeaders
         
 end
 
-
 end
 
diff --git a/controls/model/quad_files/.gitignore b/controls/model/quad_files/.gitignore
deleted file mode 100644
index 1c4bc674e912af84e9c6bed639cd5780a68b7846..0000000000000000000000000000000000000000
--- a/controls/model/quad_files/.gitignore
+++ /dev/null
@@ -1,3 +0,0 @@
-# Don't track the copied files here
-*.h
-*.c
\ No newline at end of file
diff --git a/controls/model/quad_files/copy_files.bat b/controls/model/quad_files/copy_files.bat
deleted file mode 100644
index bd5f65861b103ddaf3b761a47c9bc584bb35b50b..0000000000000000000000000000000000000000
--- a/controls/model/quad_files/copy_files.bat
+++ /dev/null
@@ -1,27 +0,0 @@
-copy ..\..\..\quad\computation_graph\src\computation_graph.c computation_graph.c
-copy ..\..\..\quad\computation_graph\src\computation_graph.h computation_graph.h
-
-copy ..\..\..\quad\sw\modular_quad_pid\src\control_algorithm.h control_algorithm.h
-copy ..\..\..\quad\sw\modular_quad_pid\src\control_algorithm.c control_algorithm.c
-
-copy ..\..\..\quad\sw\modular_quad_pid\src\PID.h PID.h
-copy ..\..\..\quad\sw\modular_quad_pid\src\type_def.h type_def.h
-copy ..\..\..\quad\sw\modular_quad_pid\src\sensor_processing.h sensor_processing.h
-copy ..\..\..\quad\sw\modular_quad_pid\src\log_data.h log_data.h
-copy ..\..\..\quad\sw\modular_quad_pid\src\quadposition.h quadposition.h
-copy ..\..\..\quad\sw\modular_quad_pid\src\util.h util.h
-copy ..\..\..\quad\sw\modular_quad_pid\src\timer.h timer.h
-copy ..\..\..\groundStation\src\backend\commands.h commands.h
-copy ..\..\..\groundStation\src\backend\callbacks.h callbacks.h
-
-mkdir graph_blocks
-copy ..\..\..\quad\computation_graph\src\graph_blocks\node_constant.h graph_blocks\node_constant.h
-copy ..\..\..\quad\computation_graph\src\graph_blocks\node_constant.c graph_blocks\node_constant.c
-copy ..\..\..\quad\computation_graph\src\graph_blocks\node_add.h graph_blocks\node_add.h
-copy ..\..\..\quad\computation_graph\src\graph_blocks\node_add.c graph_blocks\node_add.c
-copy ..\..\..\quad\sw\modular_quad_pid\src\graph_blocks\node_mixer.c graph_blocks\node_mixer.c
-copy ..\..\..\quad\sw\modular_quad_pid\src\graph_blocks\node_mixer.h graph_blocks\node_mixer.h
-copy ..\..\..\quad\sw\modular_quad_pid\src\graph_blocks\node_pid.c graph_blocks\node_pid.c
-copy ..\..\..\quad\sw\modular_quad_pid\src\graph_blocks\node_pid.h graph_blocks\node_pid.h
-copy ..\..\..\quad\sw\modular_quad_pid\src\graph_blocks\node_bounds.c graph_blocks\node_bounds.c
-copy ..\..\..\quad\sw\modular_quad_pid\src\graph_blocks\node_bounds.h graph_blocks\node_bounds.h
\ No newline at end of file
diff --git a/controls/model/test_model.slx b/controls/model/test_model.slx
index 56975e7eba373b73a005d9451ddcb553bd36f882..93229cbac59b18930c7e4a1158b9e2f6c53bb53c 100644
Binary files a/controls/model/test_model.slx and b/controls/model/test_model.slx differ
diff --git a/groundStation/Makefile b/groundStation/Makefile
index c6ea99f3502e19309fe8cf0800b0440ed920c3e7..b9024633ab737dd19d47c447fb11cda30624cf95 100644
--- a/groundStation/Makefile
+++ b/groundStation/Makefile
@@ -6,15 +6,10 @@ GXX=g++
 CFLAGS= -Wall -pedantic -Wextra -Werror -std=gnu99 -g -Wno-unused-parameter -Wno-unused-variable -Wno-unused-function -Wno-unused-but-set-variable
 CXXFLAGS= -Wall -pedantic -Wextra -Werror -Wno-reorder -Wno-unused-variable -std=c++0x -g
 INCLUDES = $(foreach dir, $(INCDIR), -I$(dir))
-INCDIR=inc src/vrpn src/vrpn/quat src/vrpn/build $(COMSRCDIR) $(BESRCDIR) $(CLISRCDIR) $(FESRCDIR)
-LIBS= -lpthread -lbluetooth -lvrpn -lquat -Lsrc/vrpn/build -Lsrc/vrpn/build/quat 
+INCDIR= src/vrpn src/vrpn/quat src/vrpn/build $(BESRCDIR) $(CLISRCDIR) $(FESRCDIR) ../quad/inc 
+LIBS= -lpthread -lbluetooth -lvrpn -lquat -Lsrc/vrpn/build -Lsrc/vrpn/build/quat -L../quad/lib -lquad_app -lcommands -lgraph_blocks -lcomputation_graph
 OBJDIR=obj
 
-# Common Objects
-COMSRCDIR=../common
-COMSOURCES := $(wildcard $(COMSRCDIR)/*.c )
-COMOBJECTS = $(COMSOURCES:$(COMSRCDIR)/%.c=$(OBJDIR)/%.o)
-
 # Backend Specific Variables
 BEBINARY=BackEnd
 BESRCDIR=src/backend
@@ -28,7 +23,7 @@ CLIBINARY=Cli
 CLISRCDIR=src/cli
 CLISOURCES := $(wildcard $(CLISRCDIR)/*.c)
 CLIOBJECTS = $(CLISOURCES:$(CLISRCDIR)/%.c=$(OBJDIR)/%.o)
-SYMLINKS=monitor setpid getpid setsetpoint getsetpoint
+SYMLINKS= getnodes addnode getparam setparam getsource setsource getoutput
 
 # Frontend-common stuff
 FESRCDIR=src/frontend
@@ -36,10 +31,13 @@ FECSOURCES := $(wildcard $(FESRCDIR)/*.c )
 FECOBJECTS = $(FECSOURCES:$(FESRCDIR)/%.c=$(OBJDIR)/%.o)
 
 
-OBJECTS= $(COMOBJECTS) $(CLIOBJECTS) $(BECOBJECTS) $(BECPPOBJECTS) $(FECOBJECTS)
+OBJECTS=$(CLIOBJECTS) $(BECOBJECTS) $(BECPPOBJECTS) $(FECOBJECTS)
 
 # Default target
-all: logs objdir backend cli $(SYMLINKS) frontend.a
+all: quad logs objdir backend cli $(SYMLINKS) frontend.a
+
+quad:
+	$(MAKE) -C ../quad
 
 $(SYMLINKS): $(CLIBINARY)
 	$(foreach symlink, $(SYMLINKS), ln -s $(CLIBINARY) $(symlink);)
@@ -55,13 +53,9 @@ cli:  $(CLIOBJECTS) $(FECOBJECTS)
 $(CLIOBJECTS) : $(OBJDIR)/%.o : $(CLISRCDIR)/%.c
 	$(GCC)  $(CFLAGS) -c $^ -o $@ $(INCLUDES) $(LIBS)
 
-
-backend: $(COMOBJECTS) $(BECPPOBJECTS) $(BECOBJECTS)
+backend: $(BECPPOBJECTS) $(BECOBJECTS)
 	$(GXX) $(CXXFLAGS) $^ -o $(BEBINARY) $(INCLUDES) $(LIBS)
 
-$(COMOBJECTS) : $(OBJDIR)/%.o : $(COMSRCDIR)/%.c
-	$(GCC)  $(CFLAGS) -c $^ -o $@ $(INCLUDES) $(LIBS)
-
 $(FECOBJECTS) : $(OBJDIR)/%.o : $(FESRCDIR)/%.c
 	$(GCC)  $(CFLAGS) -c $^ -o $@ $(INCLUDES) $(LIBS)
 
@@ -85,7 +79,6 @@ clean_logs:
 	rm -f logs/*
 
 clean:
-	rm -rf $(OBJDIR)/ $(BEBINARY) $(CLIBINARY)
+	rm -rf $(OBJDIR)/ $(BEBINARY) $(CLIBINARY) $(SYMLINKS)
 
 debug:
-	@echo $(COMOBJECTS)
diff --git a/groundStation/src/backend/backend.c b/groundStation/src/backend/backend.c
index cdd0727a269d0d857fc13be51f6a2e04696c21ae..5115a25c9919f6fd2a1422d5c5aa962d334d93de 100644
--- a/groundStation/src/backend/backend.c
+++ b/groundStation/src/backend/backend.c
@@ -29,7 +29,7 @@
 #include <netinet/tcp.h>
 
 //user created includes
-#include "../../../common/commands.h"
+#include "commands.h"
 #include "vrpn_tracker.hpp"
 #include "type_def.h"
 #include "packet.h"
@@ -38,6 +38,7 @@
 #include "config.h"
 #include "source.h"
 #include "output.h"
+#include "nodes.h"
 #include "bitwise.h"
 
 #define QUAD_BT_ADDR  "00:06:66:64:61:D6"
@@ -81,7 +82,8 @@ static void quad_recv();
 static int wasDisconnected(int fd);
 /* handle controller responses from quad to frontend */
 static void handleResponse(struct metadata *m, uint8_t * data);
-
+/* Create new dynamic logfile name */
+char * create_log_name(char * buffer, size_t max);
 
 /* Thread-safe wrappers */
 pthread_mutex_t quadSocketMutex;
@@ -112,14 +114,13 @@ fd_set rfds_master;
 int max_fd = 0;
 
 static FILE * quadlog_file;
+static int quadlog_file_open;
+static char user_specified_log_name[256] = "";
 
 pthread_mutex_t quadResponseMutex, cliInputMutex ;
 unsigned char *commandBuf;
 int newQuadResponse = 0, newCliInput = 0;
 
-// Structures to be used throughout
-modular_structs_t structs;
-
 // Callback to be ran whenever the tracker receives data.
 // Currently doing much more than it should. It will be slimmed down 
 // 		in the future.
@@ -133,8 +134,6 @@ int main(int argc, char **argv)
 {
 	int activity;
 	FD_ZERO(&rfds_master);
-	char log_file[256] = "logs/";
-
 	/* 
 	 * Create backend listening socket
 	 */
@@ -153,6 +152,8 @@ int main(int argc, char **argv)
 		err(-1, "socket");
 	}
 
+	printf("backendSocket = %d\n", backendSocket);
+
 	/* Create sockaddr and bind */
 	struct sockaddr_un sa;
 	sa.sun_family = AF_UNIX;
@@ -191,6 +192,8 @@ int main(int argc, char **argv)
 		exit(1);
 	}
 
+	printf("zyboSocket = %d\n", zyboSocket);
+
 	if (pthread_mutex_unlock(&quadSocketMutex)) {
 		err(-2, "pthrtead_mutex_unlock (%s:%d):", __FILE__, __LINE__);
 	}
@@ -205,40 +208,27 @@ int main(int argc, char **argv)
 
 	if(argc >= 2)
 	{
-		strncat(log_file, argv[1], strlen(argv[1]));
-	} else {
-		time_t rawtime;
-		char timestr [30];
-		time(&rawtime);
-		sprintf(timestr,"%s",ctime(&rawtime));
-		// Lets convert space to _ in
-		char *p = timestr;
-		size_t i = 0;
-		while(i < strlen(timestr))
-		{ 
-		    if (*p == ' ')
-		          *p = '_';
-			i++;
-			p++;
-		}
-		// timestr has a weird char at the end of it.
-		// we will not include it in our file name
-		strncat(log_file, timestr, strlen(timestr) -1 );
-		strcat(log_file, ".txt");
+		strncat(user_specified_log_name, argv[1], strlen(argv[1]));
 	}
 
+	char log_file[256];
+	create_log_name(log_file, 256);
+
 	printf("Creating log file '%s'...\n",log_file);
 	quadlog_file = fopen(log_file, "a");
+	quadlog_file_open = 1;
 
 	// Tell the quad we are ready to send it vrpn data
 	sendStartPacket();
 
-
 	if(!getenv(NOVRPN_ENV)){
+		printf("Creating VRPN tracker...\n");
 		// create vrpnTracker instance
 		tracker = ucart_vrpn_tracker_createInstance(TRACKER_IP);
 		// this function will be called whenever tracker receives data
 		ucart_vrpn_tracker_addCallback(tracker, cb);	
+	} else {
+		printf("Ignoring VRPN information...\n");
 	}
 
 	struct timeval timeout = {
@@ -358,28 +348,48 @@ void printVrpnData(struct ucart_vrpn_TrackerData * td) {
 
 int connectToZybo() {
 	int sock;
-	int status = 0;
+	int status = -1;
 
 	if (getenv(NOQUAD_ENV)) {
 		return 0;
 	}
 
-	/* Use bluetooth by default */
-	if (!getenv(QUAD_WIFI_ENV)) {
-		printf("Using BT Settings\n");
-		struct sockaddr_rc addr;
+	/* Use wifi by default */
+	if (getenv(QUAD_COMM_ENV)) {
+		/* Determine if we are using bluetooth or local */
+		if(strcmp(getenv(QUAD_COMM_ENV), "local") == 0) {
+			printf("Using Local Socket Settings\n");
+			
+			struct sockaddr_un remote;
+			char str[100];
 
-		// allocate a socket	
-		sock = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);
+			if ((sock = socket(AF_UNIX, SOCK_STREAM, 0)) == -1) {
+				perror("socket");
+				exit(1);
+			}
 
-		//set the connection params ie. who to connect to	
-		addr.rc_family = AF_BLUETOOTH;
-		addr.rc_channel = (uint8_t) QUAD_BT_CHANNEL;
-		str2ba( QUAD_BT_ADDR, &addr.rc_bdaddr );
-		
-		printf("Attempting to connect to zybo. Please be patient...\n");
-		// blocking call to connect to socket sock ie. zybo board
-		status = connect(sock, (struct sockaddr *)&addr, sizeof(addr));
+			remote.sun_family = AF_UNIX;
+			char * sock_env = getenv(QUAD_LOCAL_SOCKET);
+			strcpy(remote.sun_path, sock_env ? sock_env : QUAD_DEFAULT_LOCAL_SOCKET);
+			printf("Attempting to connect to local socket at '%s'. please be patiend.\n", remote.sun_path);
+
+			status = connect(sock, (struct sockaddr *)&remote, sizeof(remote));
+		} else if (strcmp(getenv(QUAD_COMM_ENV), "bluetooth") == 0) {
+			printf("Using BT Settings\n");
+			struct sockaddr_rc addr;
+
+			// allocate a socket	
+			sock = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);
+
+			//set the connection params ie. who to connect to	
+			addr.rc_family = AF_BLUETOOTH;
+			addr.rc_channel = (uint8_t) QUAD_BT_CHANNEL;
+			str2ba( QUAD_BT_ADDR, &addr.rc_bdaddr );
+			
+			printf("Attempting to connect to zybo. Please be patient...\n");
+			// blocking call to connect to socket sock ie. zybo board
+			status = connect(sock, (struct sockaddr *)&addr, sizeof(addr));
+		}
 	} else {
 		printf("Using WIFI settings\n");
 		struct sockaddr_in addr;
@@ -629,15 +639,18 @@ static void client_recv(int fd) {
 
 		char * first_word;
 		char * tmp = strdup(buffer);
+		// printf("tmpbuff = '%s'\n", tmp);
 		first_word = strtok(tmp, " ");
-		free(tmp);
-
+		// printf("first word = '%s'\n", first_word);
+	
 		ssize_t msg_type, i;
 		for (i = 0; i < MAX_TYPE_ID; ++i) {
 			if ((msg_type = findCommand(first_word)) != -1)
 				break;
 		}
 
+		free(tmp);
+
 		if (msg_type == -1) {
 			/* buffer was not a quad command, handling internally to
 			 * backend instead of forwarding to quad
@@ -675,10 +688,12 @@ static void client_recv(int fd) {
 		} else {
 			uint8_t packet[64];
 			struct metadata m;
-			uint8_t data[128];
+			uint8_t *data = malloc(sizeof(*data) * 128);
 			ssize_t result;  
 			ssize_t psize;
 
+			printf(" found a msg_type of %ld\n", msg_type);
+
 			switch (msg_type) {
 				case SETPARAM_ID:
 					result = EncodeSetParam(&m, data, 128, buffer);
@@ -696,8 +711,10 @@ static void client_recv(int fd) {
 					result = EncodeGetOutput(&m, data, 128, buffer);
 					break;
 				case GETNODES_ID:
-					result = -1;
-					// result = EncodeGetnodes(&m, data, 128, buffer);
+					result = EncodeGetNodes(&m, data, 128, buffer);
+					break;
+				case ADDNODE_ID:
+					result = EncodeAddNode(&m, data, 128, buffer);
 					break;
 				default:
 					result = -1;
@@ -706,6 +723,7 @@ static void client_recv(int fd) {
 
 			if (result < 0) {
 				warnx("Big problems. client_recv. EncodeMetaData");
+				free(data);
 				return;
 			}
 
@@ -713,11 +731,13 @@ static void client_recv(int fd) {
 
 			if ((psize = EncodePacket(packet, 64, &m, data)) < 0) {
 				warnx("Big problems. client_recv. EncodePacket");
+				free(data);
 				return;
 			}
 
 			/* Only add the client to the pending responses if it was a getparam command */
-			if (m.msg_type == GETPARAM_ID || m.msg_type == GETOUTPUT_ID || m.msg_type == GETSOURCE_ID) {
+			if (m.msg_type == GETPARAM_ID || m.msg_type == GETOUTPUT_ID || 
+				m.msg_type == GETSOURCE_ID || m.msg_type == GETNODES_ID || m.msg_type == ADDNODE_ID) {
 				if (clientAddPendResponses(fd, BytesTo16(packet[ID_L], packet[ID_H])) == -1) {
 					warnx("Ran out of room! Consider increasing CLIENT_MAX_PENDING_RESPONSES!\n");
 				}
@@ -729,8 +749,9 @@ static void client_recv(int fd) {
 			// }
 			// printf("'\n");
 			
-			writeQuad(packet, psize);
-
+			int retval = writeQuad(packet, psize);
+			// printf("sent %d bytes\n", retval);
+			free(data);
 		}
 
 		char * rest = &buffer[newline] + 1;
@@ -804,19 +825,33 @@ static void quad_recv() {
 
 		switch (m.msg_type) {
 			case LOG_ID:
-				/* something like this */
-				printf("(Quad) : Log found\n");
+				if (!quadlog_file_open) {
+					char log_file[256];
+					create_log_name(log_file, 256);
+					printf("New log file created: '%s'\n", log_file);
+					quadlog_file = fopen(log_file, "a");
+					quadlog_file_open = 1;
+				}
+				//printf("(Quad) : Log found\n");
 				fwrite((char *) data, sizeof(char), m.data_len, quadlog_file);
-				// fflush(quadlog_file);
 				break;
 			case RESPPARAM_ID:
 			case RESPSOURCE_ID:
 			case RESPOUTPUT_ID:
+			case RESPNODES_ID:
+			case RESPADDNODE_ID:
 				handleResponse(&m, data);
 				break;
-			case RESPNODES_ID:
+			case LOG_END_ID:
+				if (quadlog_file_open) {
+					fclose(quadlog_file);
+					quadlog_file_open = 0;	
+				}
+				printf("(Quad) : Log End found\n");
+				break;
 			default:
 				printf("(Backend): message type %d ignored from quad\n", m.msg_type);
+				break;
 		}
 	}	
 }
@@ -824,33 +859,45 @@ static void quad_recv() {
 static void handleResponse(struct metadata *m, uint8_t * data)
 {
 	ssize_t result;
-	char buffer[128];
+	char *buffer = malloc(sizeof(*buffer) * 128);
 
 	switch (m->msg_type) {
 		case RESPPARAM_ID:
-			result = DecodeResponseParam(buffer, m, data);
+			result = DecodeResponseParam(buffer, 128, m, data);
 			break;
 		case RESPSOURCE_ID:
-			result = DecodeResponseSource(buffer, m, data);
+			result = DecodeResponseSource(buffer, 128, m, data);
 			break;
 		case RESPOUTPUT_ID:
-			result = DecodeResponseOutput(buffer, m, data);
+			result = DecodeResponseOutput(buffer, 128, m, data);
+			break;
+		case RESPNODES_ID:
+			result = DecodeResponseGetNodes(buffer, 128, m, data);
+			break;
+		case RESPADDNODE_ID:
+			result = DecodeResponseAddNode(buffer, 128, m, data);
 			break;
 		default:
 			result = -1;
+			break;
 	}
 
 	if (result < 0) {
 		warnx("DecodeResponse error");
+		free(buffer);
 		return;
 	}
 
+	// printf("msg to client = '%s'\n", buffer);
+
 	for(int fd = 0; fd <= max_fd; ++fd) {
-		if (get_client_index(fd) < 0) {
+		if (get_client_index(fd) > -1) {
 			clientRemovePendResponses(fd, m->msg_id);
 			write(fd, buffer, result);
 		}
 	}
+
+	free(buffer);
 }
 
 static int wasDisconnected(int fd) {
@@ -891,3 +938,25 @@ void findTimeDiff(int respID) {
 	timeval_subtract(&result, &tend, &timeArr[respID%MAX_HASH_SIZE]);
 	printf("(BackEnd): Elapsed time = %ld ms\n", result.tv_usec/1000);
 }
+
+char * create_log_name(char * buffer, size_t max) {
+	static const char * prefix = "logs";
+	static size_t num_logs = 0;
+	static const char * format_string =  "%Y-%m-%e_%-l:%M";
+
+	time_t curr_time;
+	char c_time_string[256];
+	struct tm * tmp;
+
+	curr_time = time(NULL);
+	tmp = localtime(&curr_time);
+	strftime(c_time_string, 256, format_string, tmp);
+
+	if(strcmp(user_specified_log_name, "") == 0) {
+		sprintf(buffer, "%s/%s_%lu.txt", prefix, c_time_string, num_logs++);	
+	} else {
+		sprintf(buffer, "%s/%s_%lu.txt", prefix, user_specified_log_name, num_logs++);
+	}
+	
+	return buffer;
+}
diff --git a/groundStation/src/backend/config.h b/groundStation/src/backend/config.h
index 1ff4092641264a02aedcab0fd76080ee2e59af83..9306bd65ecf4ca80e60fb118a942e8fff00131d1 100644
--- a/groundStation/src/backend/config.h
+++ b/groundStation/src/backend/config.h
@@ -2,10 +2,7 @@
 #define __CONFIG_H
 
 
-#ifndef DEFAULT_SOCKET
-#define DEFAULT_SOCKET "/var/run/ucart.socket"
-#endif 
-
+#define DEFAULT_SOCKET "./ucart.socket"
 #define SOCKET_ENV "UCART_SOCKET"
 #define NOQUAD_ENV "UCART_NO_QUAD"
 #define NOVRPN_ENV "UCART_NO_VRPN"
@@ -14,7 +11,10 @@
 // If you are planning on using any of these env vars and you have
 // 	exported them with normal user rights. You will need to run the 
 // 	backend with sudo elevation and with the --preserve-env flag or -E
-#define QUAD_WIFI_ENV "UCART_USE_WIFI"
+#define QUAD_COMM_ENV "UCART_COMM_CHANNEL"
+
+#define QUAD_DEFAULT_LOCAL_SOCKET "./virtquad.socket"
+#define QUAD_LOCAL_SOCKET "UCART_LOCAL_SOCKET"
 #define QUAD_IP_ENV "UCART_QUAD_IP"
 #define QUAD_IP_DEFAULT "192.168.1.1"
 #define QUAD_PORT_ENV "UCART_QUAD_PORT"
diff --git a/groundStation/src/backend/nodes.c b/groundStation/src/backend/nodes.c
new file mode 100644
index 0000000000000000000000000000000000000000..695f58a00a0c67d0cdbe86db9a88c544bb6c67af
--- /dev/null
+++ b/groundStation/src/backend/nodes.c
@@ -0,0 +1,161 @@
+#include <sys/types.h>
+#include <inttypes.h>
+
+#include "nodes.h"
+#include "commands.h"
+#include "bitwise.h"
+
+
+enum GetnodeData {
+	GN_DATA_SIZE
+};
+
+
+/* Creates data and metadata for a getnodes packet.
+ * Returns data size.
+ */
+ssize_t EncodeGetNodes(
+		struct metadata *m, /* Out */
+		uint8_t *data,      /* Out */
+		size_t data_size,   /* Data buffer max size */
+		const char * msg)  /* Value is not used; only IDs */
+{
+	m->msg_type = GETNODES_ID;
+	m->data_len = GN_DATA_SIZE;
+
+	return GN_DATA_SIZE;
+}
+
+enum AddnodeData {
+	AN_TYPE_ID_L,
+	AN_TYPE_ID_H,
+	AN_NAME,
+	AN_MIN_DATA_SIZE
+};
+
+/* Creates data and metadata for a addnode packet
+ * Returns data size.
+ */
+ssize_t EncodeAddNode(
+		struct metadata * m,        /* data_len and msg_type will be populated*/
+		uint8_t * data,             /* Output buffer */
+		size_t data_size,           /* Max buffer size */
+		const char * msg)      /* Message to encode */
+{
+	m->msg_type = ADDNODE_ID;
+	
+	if (data_size < AN_MIN_DATA_SIZE) {
+		return -1;
+	}
+
+	int16_t type;
+	char name[512];
+
+	sscanf(msg, "addnode %" SCNd16 "%s\n", &type, name);
+
+	data[AN_TYPE_ID_L] = LSByte16(type);
+	data[AN_TYPE_ID_H] = MSByte16(type);
+	
+	memcpy(&data[AN_NAME], name, strlen(name) + 1); // We want to include the null byte
+	
+	m->data_len = AN_MIN_DATA_SIZE + strlen(name);
+
+	return m->data_len;
+}
+
+enum ResponseGetnodesData {
+	RESP_GN_NUM_NODES_L,
+	RESP_GN_NUM_NODES_H,
+	RESP_GN_MIN_DATA_SIZE
+};
+
+static int resizeMsg(char **msg_ptr, int *curr_max, int check_val) {
+	/* resize the msg if necessary */
+	if (*curr_max < check_val) {
+		*curr_max = *curr_max * 4;
+		*msg_ptr = realloc(*msg_ptr, sizeof(**msg_ptr) * *curr_max);
+		if (!msg_ptr) {
+			return -1;
+		}
+	}
+	return 0;
+}
+
+/* Decode a metadata and data to populate a.
+ * Returns bytes written to msg, -1 on failure.
+ */
+int DecodeResponseGetNodes(
+		char * msg, 				   /* Out */
+		size_t max_len,				   /* msg buffer max size */
+		const struct metadata *m,      /* In */
+		const uint8_t * data)	      /* In */
+{
+	/* See if we have the min. amount */
+	if (m->data_len < RESP_GN_MIN_DATA_SIZE) {
+		return -1;
+	}
+	if (m->msg_type != RESPNODES_ID) {
+		return -1;
+	}
+
+	uint16_t num_nodes = BytesTo16(data[RESP_GN_NUM_NODES_L], data[RESP_GN_NUM_NODES_H]);
+	
+	if (resizeMsg(&msg, (int *)&max_len, (m->data_len *2)) == -1) {
+		return -1;
+	}
+
+	int16_t val;
+	char name[512] = "";/* Corresponding to the maximum name len that the frontend will accept */
+	size_t i;
+	int msg_len = 0, msg_offset = 0, data_offset = RESP_GN_MIN_DATA_SIZE;
+
+	sprintf(msg, "getnodes %hu %n", num_nodes, &msg_offset);
+	msg_len += msg_offset;
+
+
+	for(i = 0; i < num_nodes * 3; ++i) {
+		if (i < num_nodes * 2) {
+			val = BytesTo16(data[data_offset], data[data_offset+1]);
+			data_offset += 2;
+			sprintf(&msg[msg_len], "%" PRId16 " %n", val, &msg_offset);
+		} else {
+			strncpy(name, (char *) (data + data_offset), 512);
+			data_offset += strlen(name) + 1;
+			sprintf(&msg[msg_len], "'%s' %n", name, &msg_offset);
+		}
+		msg_len += msg_offset;
+
+
+		if (resizeMsg(&msg, (int *)&max_len, msg_len + (msg_offset *2)) == -1) {
+			return -1;
+		}
+	}
+	strcat(&msg[msg_len], "\n");
+	return strlen(msg);
+}
+
+enum ResponseAddnodeData {
+	RESP_AN_BLOCK_ID_L,
+	RESP_AN_BLOCK_ID_H,
+	RESP_AN_DATA_SIZE
+};
+
+/* Decode a metadata and data to populate a controller.
+ * Returns bytes written to msg, -1 on failure.
+ */
+int DecodeResponseAddNode(
+		char * msg, 				   /* Out */
+		size_t max_len,				   /* msg buffer max size */
+		const struct metadata *m,      /* In */
+		const uint8_t * data)         /* In */
+{
+	if (m->data_len < RESP_AN_DATA_SIZE) {
+		return -1;
+	}
+	if (m->msg_type != RESPADDNODE_ID) {
+		return -1;
+	}
+	
+	return sprintf(msg, "addnode %d\n", 
+		BytesTo16(data[RESP_AN_BLOCK_ID_L], data[RESP_AN_BLOCK_ID_H]));
+}
\ No newline at end of file
diff --git a/groundStation/src/backend/nodes.h b/groundStation/src/backend/nodes.h
new file mode 100644
index 0000000000000000000000000000000000000000..18f57c9a35419e483253da94b3e69e2d7c657fec
--- /dev/null
+++ b/groundStation/src/backend/nodes.h
@@ -0,0 +1,47 @@
+#ifndef _nodes_h
+#define _nodes_h
+
+#include "packet.h"
+
+#include <sys/types.h>
+
+
+/* Creates data and metadata for a getnodes packet.
+ * Returns data size.
+ */
+ssize_t EncodeGetNodes(
+		struct metadata *m, /* Out */
+		uint8_t *data,      /* Out */
+		size_t data_size,   /* Data buffer max size */
+		const char * msg); /* Value is not used; only IDs */
+
+/* Creates data and metadata for a addnode packet
+ * Returns data size.
+ */
+ssize_t EncodeAddNode(
+		struct metadata * m,        /* data_len and msg_type will be populated*/
+		uint8_t * data,             /* Output buffer */
+		size_t data_size,           /* Max buffer size */
+		const char * msg);      /* Message to encode */
+
+
+/* Decode a metadata and data to populate a.
+ * Returns bytes written to msg, -1 on failure.
+ */
+int DecodeResponseGetNodes(
+		char * msg, 				   /* Out */
+		size_t max_len,				   /* msg buffer max size */
+		const struct metadata *m,      /* In */
+		const uint8_t * data);         /* In */
+
+
+/* Decode a metadata and data to populate a controller.
+ * Returns bytes written to msg, -1 on failure.
+ */
+int DecodeResponseAddNode(
+		char * msg, 				   /* Out */
+		size_t max_len,				   /* msg buffer max size */
+		const struct metadata *m,      /* In */
+		const uint8_t * data);         /* In */
+
+#endif
\ No newline at end of file
diff --git a/groundStation/src/backend/output.c b/groundStation/src/backend/output.c
index 0c3195c95b76158e657fd05f8028502bd18f5cdd..9176be6fb7678db8708f5002ac17d31c915f4acc 100644
--- a/groundStation/src/backend/output.c
+++ b/groundStation/src/backend/output.c
@@ -1,7 +1,8 @@
 #include <sys/types.h>
+#include <inttypes.h>
 
 #include "output.h"
-#include "../../../common/commands.h"
+#include "commands.h"
 #include "bitwise.h"
 
 
@@ -29,9 +30,9 @@ ssize_t EncodeGetOutput(
 		return -1;
 	}
 
-	uint16_t block, output;
+	int16_t block, output;
 
-	sscanf(msg, "getoutput %hu %hu", &block, &output);
+	sscanf(msg, "getoutput %" SCNd16 " %" SCNd16, &block, &output);
 
 	data[GO_BLOCK_ID_L] = LSByte16(block);
 	data[GO_BLOCK_ID_H] = MSByte16(block);
@@ -58,6 +59,7 @@ enum ResponseData {
  */
 int DecodeResponseOutput(
         char * msg,     /* Decoded controller message */
+		size_t max_len,				   /* msg buffer max size */
         const struct metadata * m,          /* Metadata to aid in decoding */
         const uint8_t * data)               /* Data to decode */
 {
@@ -68,7 +70,7 @@ int DecodeResponseOutput(
 		return -1;
 	}
 
-	return sprintf(msg, "getoutput %hu %hu %f\n", 
+	return snprintf(msg, max_len, "getoutput %" PRId16 " %" PRId16 " %f\n", 
 		BytesTo16(data[RESP_BLOCK_ID_L], data[RESP_BLOCK_ID_H]), 
 		BytesTo16(data[RESP_OUTPUT_ID_L], data[RESP_OUTPUT_ID_H]), 
 		BytesToFloat(data[RESP_VAL_1], data[RESP_VAL_2],
diff --git a/groundStation/src/backend/output.h b/groundStation/src/backend/output.h
index 034d6ec156d25fb27c481d08cfce1c66d7fc7f26..df923ab548c2986c7ddc1ade142aad6ade5f77b6 100644
--- a/groundStation/src/backend/output.h
+++ b/groundStation/src/backend/output.h
@@ -21,6 +21,7 @@ ssize_t EncodeGetOutput(
  */
 int DecodeResponseOutput(
 		char * msg, 				   /* Out */
+		size_t max_len,				   /* msg buffer max size */
 		const struct metadata *m,      /* In */
 		const uint8_t * data);         /* In */
 
diff --git a/groundStation/src/backend/param.c b/groundStation/src/backend/param.c
index 749960fe429f372d77f9a25c2e24029689898414..5fdba8fd5126414be4b8aee2239b3b14f0ce2205 100644
--- a/groundStation/src/backend/param.c
+++ b/groundStation/src/backend/param.c
@@ -1,7 +1,8 @@
 #include <sys/types.h>
+#include <inttypes.h>
 
 #include "param.h"
-#include "../../../common/commands.h"
+#include "commands.h"
 #include "bitwise.h"
 
 enum GetparamData {
@@ -28,8 +29,8 @@ ssize_t EncodeGetParam(
 		return -1;
 	}
 
-	uint16_t block, param;
-	sscanf(msg, "getparam %hu %hu", &block, &param);
+	int16_t block, param;
+	sscanf(msg, "getparam %" SCNd16 " %" SCNd16, &block, &param);
 
 
 	data[GP_BLOCK_ID_L] = LSByte16(block);
@@ -69,10 +70,10 @@ ssize_t EncodeSetParam(
 		return -1;
 	}
 
-	uint16_t block, param;
+	int16_t block, param;
 	float val;
 
-	sscanf(msg, "setparam %hu %hu %f", &block, &param, &val);
+	sscanf(msg, "setparam %" SCNd16 " %" SCNd16 " %f", &block, &param, &val);
 
 	data[SP_BLOCK_ID_L] = LSByte16(block);
 	data[SP_BLOCK_ID_H] = MSByte16(block);
@@ -107,7 +108,8 @@ enum ResponseData {
  * Returns bytes written to msg, -1 on failure.
  */
 int DecodeResponseParam(
-        char * msg,     /* Decoded controller message */
+        char * msg,     			/* Decoded controller message */
+		size_t max_len,				   /* msg buffer max size */
         const struct metadata * m,          /* Metadata to aid in decoding */
         const uint8_t * data)               /* Data to decode */
 {
@@ -118,7 +120,7 @@ int DecodeResponseParam(
 		return -1;
 	}
 
-	return sprintf(msg, "getparam %hu %hu %f\n", 
+	return snprintf(msg, max_len, "getparam %" PRId16 " %" PRId16 " %f\n", 
 		BytesTo16(data[RESP_BLOCK_ID_L], data[RESP_BLOCK_ID_H]), 
 		BytesTo16(data[RESP_PARAM_ID_L], data[RESP_PARAM_ID_H]), 
 		BytesToFloat(data[RESP_VAL_1], data[RESP_VAL_2],
diff --git a/groundStation/src/backend/param.h b/groundStation/src/backend/param.h
index 96ecc633290abaea67d5083be4ef8b013901af07..b8b03429e391e39b761aca2ddc77b2a41e3a580a 100644
--- a/groundStation/src/backend/param.h
+++ b/groundStation/src/backend/param.h
@@ -29,6 +29,7 @@ ssize_t EncodeSetParam(
  */
 int DecodeResponseParam(
 		char * msg, 				   /* Out */
+		size_t max_len,				   /* msg buffer max size */
 		const struct metadata *m,      /* In */
 		const uint8_t * data);         /* In */
 
diff --git a/groundStation/src/backend/source.c b/groundStation/src/backend/source.c
index dac52612d810c500644c8a5286595e090046d631..dd2540e371ee6d8b0d5aa91535e6af13f76145bf 100644
--- a/groundStation/src/backend/source.c
+++ b/groundStation/src/backend/source.c
@@ -1,7 +1,8 @@
 #include <sys/types.h>
+#include <inttypes.h>
 
 #include "source.h"
-#include "../../../common/commands.h"
+#include "commands.h"
 #include "bitwise.h"
 
 
@@ -28,9 +29,9 @@ ssize_t EncodeGetSource(
 	if (data_size < GS_DATA_SIZE) {
 		return -1;
 	}
-	uint16_t block, input;
+	int16_t block, input;
 
-	sscanf(msg, "getsource %hu %hu", &block, &input);
+	sscanf(msg, "getsource %" SCNd16 " %" SCNd16, &block, &input);
 
 	data[GS_BLOCK_ID_L] = LSByte16(block);
 	data[GS_BLOCK_ID_H] = MSByte16(block);
@@ -68,10 +69,10 @@ ssize_t EncodeSetSource(
 		return -1;
 	}
 	
-	uint16_t dst_block, dst_input;
-	uint16_t src_block, src_output;
+	int16_t dst_block, dst_input;
+	int16_t src_block, src_output;
 
-	sscanf(msg, "setsource %hu %hu %hu %hu", &dst_block, &dst_input, &src_block, &src_output);
+	sscanf(msg, "setsource %" SCNd16 " %" SCNd16 " %" SCNd16 " %" SCNd16, &dst_block, &dst_input, &src_block, &src_output);
 
 	data[SS_DST_BLOCK_ID_L] = LSByte16(dst_block);
 	data[SS_DST_BLOCK_ID_H] = MSByte16(dst_block);
@@ -88,10 +89,14 @@ ssize_t EncodeSetSource(
 
 
 enum ResponseData {
-	RESP_DST_BLOCK_ID,
-	RESP_DST_INPUT_ID,
-	RESP_SRC_BLOCK_ID,
-	RESP_SRC_OUTPUT_ID,
+	RESP_DST_BLOCK_ID_L,
+	RESP_DST_BLOCK_ID_H,
+	RESP_DST_INPUT_ID_L,
+	RESP_DST_INPUT_ID_H,
+	RESP_SRC_BLOCK_ID_L,
+	RESP_SRC_BLOCK_ID_H,
+	RESP_SRC_OUTPUT_ID_L,
+	RESP_SRC_OUTPUT_ID_H,
 	RESP_DATA_SIZE
 };
 
@@ -100,6 +105,7 @@ enum ResponseData {
  */
 int DecodeResponseSource(
         char * msg,     /* Decoded controller message */
+		size_t max_len,				   /* msg buffer max size */
         const struct metadata * m,          /* Metadata to aid in decoding */
         const uint8_t * data)               /* Data to decode */
 {
@@ -110,5 +116,9 @@ int DecodeResponseSource(
 		return -1;
 	}
 
-	return sprintf(msg, "getsource %d %d\n", data[RESP_SRC_BLOCK_ID], data[RESP_SRC_OUTPUT_ID]);
+	return snprintf(msg, max_len, "getsource %" PRId16 " %" PRId16 " %" PRId16" %" PRId16 "\n", 
+		BytesTo16(data[RESP_DST_BLOCK_ID_L], data[RESP_DST_BLOCK_ID_H]),
+		BytesTo16(data[RESP_DST_INPUT_ID_L], data[RESP_DST_INPUT_ID_H]),
+		BytesTo16(data[RESP_SRC_BLOCK_ID_L], data[RESP_SRC_BLOCK_ID_H]),
+		BytesTo16(data[RESP_SRC_OUTPUT_ID_L], data[RESP_SRC_OUTPUT_ID_H]));
 }
diff --git a/groundStation/src/backend/source.h b/groundStation/src/backend/source.h
index 577f27717b9fbce4504169bdb6e163a528cc7c34..40a78f0c3becf3074e77e2b91060f0d1a188da26 100644
--- a/groundStation/src/backend/source.h
+++ b/groundStation/src/backend/source.h
@@ -29,6 +29,7 @@ ssize_t EncodeSetSource(
  */
 int DecodeResponseSource(
 		char * msg, 				   /* Out */
+		size_t max_len,				   /* msg buffer max size */
 		const struct metadata *m,      /* In */
 		const uint8_t * data);         /* In */
 
diff --git a/groundStation/src/backend/update.c b/groundStation/src/backend/update.c
index 1f51f0be78c78b4671f2066124e15c90b96d3621..db546e05a3891ff0f9f52bc2ecdfc658012ed65b 100644
--- a/groundStation/src/backend/update.c
+++ b/groundStation/src/backend/update.c
@@ -1,5 +1,5 @@
 #include "update.h"
-#include "../../../common/commands.h"
+#include "commands.h"
 #include "bitwise.h"
 
 #include <sys/types.h>
diff --git a/groundStation/src/cli/cli.c b/groundStation/src/cli/cli.c
index acec4dbc6352ca971b84289eb24cfc918da98bbd..ebde95590a1250cb280f6e8329a81d465d8ecd65 100644
--- a/groundStation/src/cli/cli.c
+++ b/groundStation/src/cli/cli.c
@@ -52,9 +52,8 @@ int main(int argc, char **argv)
 	// recognize which cli command the user has entered
 	if (cmdID == -1) {
 		command = argv[1];
-		for(i = 0; i < MAX_COMMANDS; ++i) {
-			if (strncmp(command, commandNames[i], strlen(commandNames[i])) == 0)
-			{
+		for (i = 0; i < MAX_COMMANDS; ++i) {
+			if (strncmp(command, commandNames[i], strlen(commandNames[i])) == 0) {
 				cmdID = i;
 			}	
 		}
@@ -78,7 +77,7 @@ int main(int argc, char **argv)
 	}
 	
 	/**
-	 * I the user has asked for help, and we have already found
+	 * If the user has asked for help, and we have already found
 	 *  the command that they are trying to use. Then we need
 	 *  to be able to provide the help info wihtout the 
 	 *  requirement of the backend
@@ -100,7 +99,7 @@ int main(int argc, char **argv)
 	// Call the appropriate function
 	if (useSymlink) {
 		(*cli_functions[cmdID]) (conn, argc, &argv[0]);
-	}else {
+	} else {
 		(*cli_functions[cmdID]) (conn, argc-1, &argv[1]);
 	}
 
@@ -109,4 +108,130 @@ int main(int argc, char **argv)
 		ucart_backendDisconnect(conn);
 	}
     return 0;
+}
+
+/* This function is called by other cli functions to check for a help condition */
+int help_check(int argc, char ** argv) {
+	int c;
+	static int needHelp = 0;
+	static struct option long_options[] = {
+ 		/* These options don’t set a flag. We distinguish them by their indices. */
+ 		{"help",	no_argument,	&needHelp,	1},
+ 		{0, 0, 0, 0}
+ 	};
+
+ 	while (1)
+	{
+		/* getopt_long stores the option index here. */
+		int option_index = 0;
+
+		c = getopt_long(argc, argv, "h", long_options, &option_index);
+
+		/* Detect the end of the options. */
+		if (c == -1)
+			break;
+
+		if (c == 'h') {
+			needHelp = 1;
+		}
+	}
+	return needHelp;
+}
+
+int isNumber(char *number) {
+    int i = 0;
+    //checking for negative numbers
+    if (number[0] == '-')
+        i = 1;
+    for (; number[i] != 0; i++)
+    {
+        //if (number[i] > '9' || number[i] < '0')
+        if (!isdigit(number[i]))
+            return 0;
+    }
+    return 1;
+}
+
+int convert_to_id(struct backend_conn * conn, char **argv, struct convert_data * convert_data, int conversion_requirement) {
+	/* variables used to search for id values */
+	size_t num_nodes = 0, i;
+	struct frontend_node_data* search_data;
+	int search_1 = 0, search_2 = 0;
+	const struct graph_node_type * node_definition;
+
+		
+
+	if (!isNumber(argv[1])) {
+		search_1 = 1;
+	} else {
+		convert_data->val_1 = atoi(argv[1]);	
+	}
+
+	if (!isNumber(argv[2])) {
+		search_2 = 1;
+	} else {
+		convert_data->val_2 = atoi(argv[2]);
+	}
+
+	if (!search_1 && !search_2) {
+		return 0;
+	}
+
+	search_data = malloc(sizeof((*search_data)) * num_nodes);
+
+	if (frontend_getnodes(conn, &search_data, &num_nodes)) {
+		return 1;
+	}
+
+	if (search_1) {
+		for (i = 0; i < num_nodes; ++i) {
+			if (strncasecmp(search_data[i].name, argv[1], strlen(search_data[i].name)) == 0) {
+				convert_data->val_1 = search_data[i].block;
+				node_definition = blockDefs[search_data[i].type];
+				break;
+			}
+		}
+	}
+
+	if (i == num_nodes)
+		return 1;
+
+	if (search_2) {
+		const int  *num_s2_options;
+		const char* const* s2_options_arr;
+
+		if (!search_1) {
+			node_definition = blockDefs[search_data[convert_data->val_1].type];
+		}
+		
+		switch (conversion_requirement) {
+			case PARAM:
+				num_s2_options = &node_definition->n_params;
+				s2_options_arr = node_definition->param_names;
+				break;
+			case INPUT:
+				num_s2_options = &node_definition->n_inputs;
+				s2_options_arr = node_definition->input_names;
+				break;
+			case OUTPUT:
+				num_s2_options = &node_definition->n_outputs;
+				s2_options_arr = node_definition->output_names;
+				break;
+		}
+
+		for (i = 0; i < (size_t)*num_s2_options; ++i) {
+			if (strncasecmp(s2_options_arr[i], 
+					argv[2], 
+					strlen(s2_options_arr[i])) == 0) {
+				convert_data->val_2 = i;
+				search_2 = 0;
+				break;
+			}
+		}
+	}
+	for(i = 0; i < num_nodes; ++i) {
+		free(search_data[i].name);
+	}
+	free(search_data);
+	return search_2;
 }
\ No newline at end of file
diff --git a/groundStation/src/cli/cli.h b/groundStation/src/cli/cli.h
index 707d16836783fe34279d1b0935995fa535a67fad..8a14201024f41c39a921beea10f19e36428047af 100644
--- a/groundStation/src/cli/cli.h
+++ b/groundStation/src/cli/cli.h
@@ -1,38 +1,82 @@
-#ifndef __CLI_H
-#define __CLI_H
+#ifndef _CLI_H
+#define _CLI_H
 
-#include "cli_monitor.h"
+#include "frontend_common.h"
 #include "cli_source.h"
 #include "cli_param.h"
 #include "cli_output.h"
+#include "cli_nodes.h"
+
+struct backend_conn;
 
 enum CommandNameIds{
-	CMD_MONITOR,
 	CMD_GETSOURCE,
 	CMD_SETSOURCE,
 	CMD_GETPARAM,
 	CMD_SETPARAM,
 	CMD_GETOUTPUT,
+	CMD_GETNODES,
+	CMD_ADDNODE,
 	MAX_COMMANDS
 };
 
 typedef int (*cli_function_ptr)(struct backend_conn *, int, char **);
 static cli_function_ptr cli_functions[] = {
-	&cli_monitor,
 	&cli_getsource,
 	&cli_setsource,
 	&cli_getparam,
 	&cli_setparam,
 	&cli_getoutput,
+	&cli_getnodes,
+	&cli_addnode
 };
 
 static char* commandNames[MAX_COMMANDS] = {
-	"monitor",
 	"getsource",
 	"setsource",
 	"getparam",
 	"setparam",
-	"getoutput"
+	"getoutput",
+	"getnodes",
+	"addnode"
+};
+
+/**
+ * generic two short struct for use with 'convert_to_id()'
+ */
+struct convert_data {
+	int16_t val_1;
+	int16_t val_2;
 };
 
-#endif /* __CLI_H */
\ No newline at end of file
+enum conversion_requirements {
+	INPUT,
+	PARAM,
+	OUTPUT
+};
+/**
+ * isNumber takes a char * such as argv[n] and checks to see if it is a number.
+ * @param  number  null terminating string (ex. argv[1])
+ * @return         1 if 'number' is a number
+ *                 0 if 'number' is not a number 
+ */
+int isNumber(char *number);
+
+/**
+ * convert_to_id will take in the convert_data struct and either use the values from argv or 
+ * 	find the correct values by using the optional strings passed in from argv
+ * @param  conn         backend connection struct
+ * @param  argv         argv from the cmd line
+ * @param  convert_data saves the necissary converted data in this struct for later use.
+ * @return             	returns an error integer
+ */
+int convert_to_id(struct backend_conn * conn, char **argv, struct convert_data * convert_data, int conversion_requirement);
+
+/* This function is called by other cli functions to check for a help condition */
+/**
+ * determine if the user asked for help
+ * @return      1 if help is needed, else 0.
+ */
+int help_check(int argc, char ** argv);
+
+#endif /* _CLI_H */
\ No newline at end of file
diff --git a/groundStation/src/cli/cli_getpid.c b/groundStation/src/cli/cli_getpid.c
deleted file mode 100644
index 4bf634e3e2e966ea59ac54d8510c3607815f462d..0000000000000000000000000000000000000000
--- a/groundStation/src/cli/cli_getpid.c
+++ /dev/null
@@ -1,176 +0,0 @@
-#include <stdio.h>
-#include <unistd.h>
-#include <getopt.h>
-
-#include "cli_getpid.h"
-
-int cli_getpid(struct backend_conn * conn,	int argc, char **argv) {
-	int c;
-	static int getRoll = 0, getPitch = 0, getYaw = 0, getAll = 0;
-	static int getRollV = 0, getPitchV = 0, getYawV = 0;
-	static int getHeight = 0, getLat = 0, getLong = 0;
-	static int needHelp = 0;
-	struct frontend_pid_data pid_data;
-	static struct option long_options[] = {
- 		/* These options don’t set a flag. We distinguish them by their indices. */
- 		{"roll",	no_argument,	&getRoll,	1},
- 		{"pitch",   no_argument,   	&getPitch, 	1},
- 		{"yaw",   no_argument,   	&getYaw, 	1},
- 		{"rollv",   no_argument,   	&getRollV, 	1},
- 		{"pitchv",   no_argument,   &getPitchV, 1},
- 		{"yawv",   no_argument,   	&getYawV, 	1},
- 		{"height",   no_argument,   &getHeight,	1},
- 		{"lat",   no_argument,   	&getLat, 	1},
- 		{"long",   no_argument,   	&getLong, 	1},
- 		{"help",	no_argument,	&needHelp,	1},
- 		{0, 0, 0, 0}
- 	};
-
- 	while (1)
-	{
-		/* getopt_long stores the option index here. */
-		int option_index = 0;
-
-		c = getopt_long(argc, argv, "a", long_options, &option_index);
-
-		/* Detect the end of the options. */
-		if (c == -1)
-			break;
-
-		if (c == 'a') {
-			getAll = 1;
-		}
-	}
-
-	if (needHelp) {
-		printf("Getpid gets the p, i , and d constant values of any single controller\n");
-		printf("Usage Syntax : \n\t./Cli getpid controller [options...]\n");
-		printf("Symlink Usage Syntax : \n\t./getpid controller [options...]\n\n");
-		printf("Available 'controllers' include the following\n");
-		printf("\t[--pitch] : Pitch\n\t[--roll] : Roll\n\t[--yaw] : Yaw\n");
-		printf("\t[--pitchv] : Pitch Velocity\n\t[--rollv] : Roll Velocity\n\t[--yawv] : Yaw Velocity\n");
-		printf("\t[--height] : Z\n\t[--lat] : X\n\t[--long] : Y\n\n");
-		return 0;
-	}
-
-	if (argc < 2) {
-		printf("Incorrect Usage, run './cli getpid --help' for correct usage.\n");
-		return 1;
-	}
-	
-	int result;
-	if(getAll) {
-		for(int i = 0; i < PID_NUM_PIDS; ++i) {
-			pid_data.controller = i;
-			if ((result = getPidValues(conn, &pid_data))) {
-				return result;
-			}
-		}
-	} else {
-		if(getPitch) {
-			pid_data.controller = PID_PITCH;
-			if ((result = getPidValues(conn, &pid_data))) {
-				return result;
-			}
-		}
-		if(getRoll) {
-			pid_data.controller = PID_ROLL;
-			if ((result = getPidValues(conn, &pid_data))) {
-				return result;
-			}
-		}
-		if(getYaw) {
-			pid_data.controller = PID_YAW;
-			if ((result = getPidValues(conn, &pid_data))) {
-				return result;
-			}
-		}
-		if(getPitchV) {
-			pid_data.controller = PID_PITCH_RATE;
-			if ((result = getPidValues(conn, &pid_data))) {
-				return result;
-			}
-		}
-		if(getRollV) {
-			pid_data.controller = PID_ROLL_RATE;
-			if ((result = getPidValues(conn, &pid_data))) {
-				return result;
-			}
-		}
-		if(getYawV) {
-			pid_data.controller = PID_YAW_RATE;
-			if ((result = getPidValues(conn, &pid_data))) {
-				return result;
-			}
-		}
-		if(getHeight) {
-			pid_data.controller = PID_HEIGHT;
-			if ((result = getPidValues(conn, &pid_data))) {
-				return result;
-			}
-		}
-		if(getLat) {
-			pid_data.controller = PID_LAT;
-			if ((result = getPidValues(conn, &pid_data))) {
-				return result;
-			}
-		}
-		if(getLong) {
-			pid_data.controller = PID_LONG;
-			if ((result = getPidValues(conn, &pid_data))) {
-				return result;
-			}
-		}
-
-	}
-
-	return 0;
-}
-
-int getPidValues(struct backend_conn * conn, struct frontend_pid_data * pid_data) {
-	if(frontend_getpid(conn, pid_data)) {
-		return 1;
-	}
-
-	switch(pid_data->controller) {
-		case PID_PITCH :
-			printf("Pitch Constants: P = %f\tI = %f\tD = %f\n",
-				pid_data->p, pid_data->i, pid_data->d);
-			break;
-		case PID_ROLL :
-			printf("Roll Constants: P = %f\tI = %f\tD = %f\n",
-				pid_data->p, pid_data->i, pid_data->d);
-			break;
-		case PID_YAW :
-			printf("Yaw Constants: P = %f\tI = %f\tD = %f\n",
-				pid_data->p, pid_data->i, pid_data->d);
-			break;
-		case PID_PITCH_RATE :
-			printf("Pitch Rate Constants: P = %f\tI = %f\tD = %f\n",
-				pid_data->p, pid_data->i, pid_data->d);
-			break;
-		case PID_ROLL_RATE :
-			printf("Roll Rate Constants: P = %f\tI = %f\tD = %f\n",
-				pid_data->p, pid_data->i, pid_data->d);
-			break;
-		case PID_YAW_RATE :
-			printf("Yaw Rate Constants: P = %f\tI = %f\tD = %f\n",
-				pid_data->p, pid_data->i, pid_data->d);
-			break;
-		case PID_HEIGHT :
-			printf("Height Constants: P = %f\tI = %f\tD = %f\n",
-				pid_data->p, pid_data->i, pid_data->d);
-			break;
-		case PID_LAT :
-			printf("Latitude Constants: P = %f\tI = %f\tD = %f\n",
-				pid_data->p, pid_data->i, pid_data->d);
-			break;
-		case PID_LONG :
-			printf("Longitude Constants: P = %f\tI = %f\tD = %f\n",
-				pid_data->p, pid_data->i, pid_data->d);
-			break;
-		default :
-			break;
-	}
-	return 0;
-}
diff --git a/groundStation/src/cli/cli_getpid.h b/groundStation/src/cli/cli_getpid.h
deleted file mode 100644
index 799681f06d114602083e304f2cba321891155fbc..0000000000000000000000000000000000000000
--- a/groundStation/src/cli/cli_getpid.h
+++ /dev/null
@@ -1,9 +0,0 @@
-#ifndef __CLI_GETPID_H
-#define __CLI_GETPID_H
-
-#include "frontend_getpid.h"
-
-int getPidValues(struct backend_conn *, struct frontend_pid_data *);
-int cli_getpid(struct backend_conn * conn, int argc, char ** argv);
-
-#endif /* __CLI_GETPID_H */
\ No newline at end of file
diff --git a/groundStation/src/cli/cli_getsetpoint.c b/groundStation/src/cli/cli_getsetpoint.c
deleted file mode 100644
index 8cb30bb15b14fa643f8bf9ce60c4f1268c36a233..0000000000000000000000000000000000000000
--- a/groundStation/src/cli/cli_getsetpoint.c
+++ /dev/null
@@ -1,134 +0,0 @@
-#include <stdio.h>
-#include <unistd.h>
-#include <getopt.h>
-
-#include "cli_getsetpoint.h"
-
-int cli_getsetpoint(struct backend_conn * conn,	int argc, char **argv) {
-	int c;
-	static int getheight = 0, getlat = 0, getlong = 0;
-	static int getpitch = 0, getroll = 0, getyaw = 0;
-	struct frontend_setpoint_data setpoint_data;
-	static int needHelp = 0;
-	static int mask;
-	static struct option long_options[] = {
- 		/* These options don’t set a flag. We distinguish them by their indices. */
- 		{"help",	no_argument,	&needHelp,	1},
- 		{"height",  no_argument,	&getheight, 1},
- 		{"long",	no_argument,	&getlong,	1},
- 		{"lat",		no_argument, 	&getlat,	1},
- 		{"pitch",   no_argument,	&getpitch,  1},
- 		{"roll",	no_argument,	&getroll,	1},
- 		{"yaw",		no_argument, 	&getyaw,	1},
- 		{0, 0, 0, 0}
- 	};
-
- 	while (1)
-	{
-		/* getopt_long stores the option index here. */
-		int option_index = 0;
-
-		c = getopt_long(argc, argv, "a", long_options, &option_index);
-
-		/* Detect the end of the options. */
-		if (c == -1)
-			break;
-
-		if (c == 'a') {
-			getheight = 1;
-			getlat = 1;
-			getlong = 1;
-			getpitch = 1;
-			getroll = 1;
-			getyaw = 1;
-		}
-	}
-
-	if (needHelp) {
-		printf("Getsetpoint gets the height, lat, long, pitch, roll and yaw set point values for the quad.\n");
-		printf("Usage Syntax : \n\t./Cli getsetpoint [options...]\n");
-		printf("Symlink Usage Syntax : \n\t./getsetpoint [options...]\n\n");
-		printf("Available options include the following\n");
-		printf("\t[-a] 		 : Gets all of the following setpoints\n");
-		printf("\t[--height] : Gets the height setpoint\n");
-		printf("\t[--lat]    : Gets the latitude setpoint\n");
-		printf("\t[--long]   : Gets the longitude setpoint\n");
-		printf("\t[--pitch]  : Gets the pitch setpoint\n");
-		printf("\t[--roll]   : Gets the roll setpoint\n");
-		printf("\t[--yaw]    : Gets the yaw setpoint\n");
-		return 0;
-	}
-
-	if (argc < 2) {
-		printf("Incorrect Usage, run './cli getsetpoint --help' for correct usage.\n");
-		return 1;
-	}
-	int result;
-	if(getheight) {
-		if ((result = getSetPointValues(conn, &setpoint_data, HEIGHT))) {
-			return result;
-		}
-	}
-	if(getlat) {
-		if ((result = getSetPointValues(conn, &setpoint_data, LAT))) {
-			return result;
-		}
-	}
-	if(getlong) {
-		if ((result = getSetPointValues(conn, &setpoint_data, LONGG))) {
-			return result;
-		}
-	}
-	if(getpitch) {
-		if ((result = getSetPointValues(conn, &setpoint_data, PITCH))) {
-			return result;
-		}
-	}
-	if(getroll) {
-		if ((result = getSetPointValues(conn, &setpoint_data, ROLL))) {
-			return result;
-		}
-	}
-	if(getyaw) {
-		if ((result = getSetPointValues(conn, &setpoint_data, YAW))) {
-			return result;
-		}
-	}
-
-	return 0;
-}
-
-int getSetPointValues(struct backend_conn * conn, struct frontend_setpoint_data * setpoint_data, int type) {
-	if(frontend_getsetpoint(conn, setpoint_data, type)) {
-		return 1;
-	}
-
-	switch(type) {
-		case HEIGHT :
-			printf("Height: %f\n",
-				setpoint_data->height);
-			break;
-		case LAT :
-			printf("Latitude: %f\n",
-				setpoint_data->lat);
-		case LONGG :
-			printf("Longitude: %f\n",
-				setpoint_data->longg);
-			break;
-		case PITCH :
-			printf("Pitch: %f\n",
-				setpoint_data->pitch);
-			break;
-		case ROLL :
-			printf("Roll: %f\n",
-				setpoint_data->roll);
-			break;
-		case YAW :
-			printf("Yaw: %f\n",
-				setpoint_data->yaw);
-			break;
-		default :
-			break;
-	}
-	return 0;
-}
diff --git a/groundStation/src/cli/cli_getsetpoint.h b/groundStation/src/cli/cli_getsetpoint.h
deleted file mode 100644
index a852b37973c84b21ca985e89dbb90180f35907ba..0000000000000000000000000000000000000000
--- a/groundStation/src/cli/cli_getsetpoint.h
+++ /dev/null
@@ -1,18 +0,0 @@
-#ifndef __CLI_GETSETPOINT_H
-#define __CLI_GETSETPOINT_H
-
-#include "frontend_getsetpoint.h"
-
-enum {
-	HEIGHT,
-	LAT,
-	LONGG,
-	PITCH,
-	ROLL,
-	YAW
-};
-
-int getSetPointValues(struct backend_conn *, struct frontend_setpoint_data *, int type);
-int cli_getsetpoint(struct backend_conn * conn, int argc, char ** argv);
-
-#endif /* __CLI_GETSETPOINT_H */
\ No newline at end of file
diff --git a/groundStation/src/cli/cli_monitor.c b/groundStation/src/cli/cli_monitor.c
deleted file mode 100644
index 7f1df5bf38236b1a6e88224d25b9ac57e69bc9af..0000000000000000000000000000000000000000
--- a/groundStation/src/cli/cli_monitor.c
+++ /dev/null
@@ -1,151 +0,0 @@
-#define _GNU_SOURCE
-
-#include <stdio.h>
-#include <unistd.h>
-#include <getopt.h>
-#include <time.h>
-#include <unistd.h>
-#include <err.h>
-
-#include "cli_monitor.h"
-#include "frontend_tracker.h"
-#include "frontend_getpid.h"
-#include "pid_common.h"
-
-int rate = 10;
-static int pidcounter = 0;
-static int monitorcounter = 0;
-
-int cli_monitor(struct backend_conn * conn,	int argc, char **argv) {
-	int c, result;
-	int countFlag = 0;
-	int count;
-	int forever = 0;
-	static int needHelp = 0;
-
-	static struct option long_options[] = {
- 		/* These options don’t set a flag. We distinguish them by their indices. */
- 		{"help",	no_argument,	&needHelp,	1},
- 		{0, 0, 0, 0}
- 	};
-
- 	while (1)
-	{
-		/* getopt_long stores the option index here. */
-		int option_index = 0;
-		
-		// If you change this 		  VVV 		please also update the help message 
-		c = getopt_long(argc, argv, "fc:r:", long_options, &option_index);
-
-		if (c == -1)
-			break;
-
- 		switch(c) {
-			case 'c' :
-				count = atoi(optarg);
-				countFlag = 1;
-				break;
-			case 'r' :
-				rate = atoi(optarg) + 1;
-				break;
-			case 'f' :
-				forever = 1;
-				break;
-			default :
-				break;
-		}
-	}
-	
-	if (needHelp) {
-		printf("Monitor provides real time information about the quad. Including positional data as well as controller constants\n\n");
-		printf("Usage Syntax : \n\t./Cli monitor [options...]\n");
-		printf("Symlink Usage Syntax : \n\t./monitor [options...]\n\n");
-		printf("Available options include the following\n");
-		printf("\t[-f] : Run monitor continuously until you kill the program. (ctrl-C)\n");
-		printf("\t[-c] 'count' : Sets the number of times the monitor will refresh\n");
-		printf("\t[-r] 'rate' : Sets the 'rate' at which the monitor will refresh per second\n");
-		return 0;
-	}
-
-	if (forever) {
-		for (;;) {
-			struct timespec req;
-			if (rate == 1) {
-				req.tv_sec = 1;
-				req.tv_nsec = 0;
-			} else { 
-				req.tv_sec = 0;
-				req.tv_nsec = 1000000000 / rate;
-			}
-			nanosleep(&req, NULL);
-			monitor(conn);
-		}
-	} else if (countFlag) {
-		for (int i = 0; i < count; i++) {
-			result = monitor(conn);
-
-			struct timespec req;
-			if (rate == 1) {
-				req.tv_sec = 1;
-				req.tv_nsec = 0;
-			} else { 
-				req.tv_sec = 0;
-				req.tv_nsec = 1000000000 / rate;
-			}
-			nanosleep(&req, NULL);
-		}
-	} else {
-		return monitor(conn);
-	}
-
-	return result;
-}
-
-int monitor(struct backend_conn * conn) {
-	static struct frontend_pid_data pid_data[PID_NUM_PIDS];
-	/* Get tracker data */
-	struct frontend_tracker_data td;
-
-	if (frontend_track(conn, &td)) {
-		errx(1, "Error reading tracker data");
-	}
-
-	if (monitorcounter == 0) {
-		for(int i = 0; i < PID_NUM_PIDS; ++i) {
-			pid_data[i].controller = i;
-			if (frontend_getpid(conn, &pid_data[i])) {
-				errx(1, "Error reading pid values");
-			}
-		}
-	} else {
-		if ((monitorcounter  % (rate * 5)) == 0) {
-			pid_data[pidcounter].controller = pidcounter;
-			if (frontend_getpid(conn, &pid_data[pidcounter])) {
-				errx(1, "Error reading pid values");
-			}
-		}
-	}
-	
-	/* Print stuff on screen */
-	/* Assuming a tab width of 8 columns */
-	printf("\033[2J");
-	printf("STATUS: NA\n");
-	printf("CTRLR :      P          R          Y          P_V        R_V        Y_V        H          LAT        LON\n");
-	printf("  P   :%10.3lf %10.3lf %10.3lf %10.3lf %10.3lf %10.3lf %10.3lf %10.3lf %10.3lf\n",
-			pid_data[0].p, pid_data[1].p, pid_data[2].p, pid_data[3].p, pid_data[4].p,
-		 	pid_data[5].p, pid_data[6].p, pid_data[7].p, pid_data[8].p);
-	printf("  I   :%10.3lf %10.3lf %10.3lf %10.3lf %10.3lf %10.3lf %10.3lf %10.3lf %10.3lf\n",
-			pid_data[0].i, pid_data[1].i, pid_data[2].i, pid_data[3].i, pid_data[4].i,
-		 	pid_data[5].i, pid_data[6].i, pid_data[7].i, pid_data[8].i);
-	printf("  D   :%10.3lf %10.3lf %10.3lf %10.3lf %10.3lf %10.3lf %10.3lf %10.3lf %10.3lf\n",
-			pid_data[0].d, pid_data[1].d, pid_data[2].d, pid_data[3].d, pid_data[4].d,
-		 	pid_data[5].d, pid_data[6].d, pid_data[7].d, pid_data[8].d);
-	printf("PosAtt:      H         LAT       LON       P         R         Y\n");
-	printf("      :%10.3lf%10.3lf%10.3lf%10.3lf%10.3lf%10.3lf\n",
-			td.height, td.lateral, td.longitudinal,
-			td.pitch, td.roll, td.yaw); //TODO: Tracker data long and lat are backwards
-	
-	monitorcounter++;
-	pidcounter = ((pidcounter + 1)  % PID_NUM_PIDS);
-	return 0;
-}
diff --git a/groundStation/src/cli/cli_monitor.h b/groundStation/src/cli/cli_monitor.h
deleted file mode 100644
index 694323739001d0b0e1b820c37b87ff95362f27e5..0000000000000000000000000000000000000000
--- a/groundStation/src/cli/cli_monitor.h
+++ /dev/null
@@ -1,14 +0,0 @@
-#ifndef __CLI_MONITOR_H
-#define __CLI_MONITOR_H
-
-#include <time.h>
-
-#include "frontend_getpid.h"
-
-#define SECOND_IN_NANO 1000000000
-
-int cli_monitor(struct backend_conn * conn,	int argc, char **argv);
-
-// Return 0 on success and 1 otherwise
-int monitor(struct backend_conn * conn);
-#endif /* __CLI_MONITOR_H */
\ No newline at end of file
diff --git a/groundStation/src/cli/cli_nodes.c b/groundStation/src/cli/cli_nodes.c
new file mode 100644
index 0000000000000000000000000000000000000000..281ee6fb60dfd66b29c60515748a7fc6c908a344
--- /dev/null
+++ b/groundStation/src/cli/cli_nodes.c
@@ -0,0 +1,103 @@
+#include <stdio.h>
+#include <string.h>
+#include <unistd.h>
+#include <inttypes.h>
+
+#include "cli.h"
+#include "cli_nodes.h"
+#include "frontend_nodes.h"
+#include "graph_blocks.h"
+
+
+int cli_getnodes(struct backend_conn * conn, int argc, char ** argv) {
+	size_t num_nodes = 0;
+	struct frontend_node_data * node_data;
+
+	int needHelp = 0;
+	
+	size_t i, n;
+
+
+	if ((needHelp = help_check(argc, argv))) {
+		printf("getnodes gets the number of nodes along with the block_id, type_id and name of each node\n");
+		printf("Usage Syntax : \n\t./Cli getnodes\n");
+		printf("Symlink Usage Syntax : \n\t./getnodes\n\n");
+		return 0;
+	}
+
+	if (argc > 1) {
+		printf("Incorrect Usage, run './cli getnodes --help' for correct usage.\n");
+		return 1;
+	}
+
+	node_data = malloc(sizeof(*node_data));
+
+	if (frontend_getnodes(conn, &node_data, &num_nodes)) {
+		free(node_data);
+		return 1;
+	}
+
+	printf("--------------------------------------------------------------------------------------------------------------\n");
+	printf("The following %lu Nodes have been found:\n", num_nodes);
+	printf("--------------------------------------------------------------------------------------------------------------\n");
+	printf("\tBLOCK\t\tTYPE\t\tNAME\t\tINPUTS\t\tPARAMS\t\tOUTPUTS\n");
+	printf("--------------------------------------------------------------------------------------------------------------\n");
+	for(i = 0; i < num_nodes; ++i) {
+		printf("\t%3d\t\t%3d\t\t%s\t", node_data[i].block, node_data[i].type, node_data[i].name);
+		
+		const struct graph_node_type * node_definition = blockDefs[node_data[i].type];
+		printf(" {");
+
+		for(n = 0; n < (size_t)node_definition->n_inputs; ++n) {
+			printf(" %-5s ", node_definition->input_names[n]);
+		}
+		printf("} ");
+
+		printf(" {");
+
+		for(n = 0; n < (size_t)node_definition->n_params; ++n) {
+			printf(" %s ", node_definition->param_names[n]);
+		}
+		printf("} ");
+
+		printf(" {");
+
+		for(n = 0; n < (size_t)node_definition->n_outputs; ++n) {
+			printf(" %s ", node_definition->output_names[n]);
+		}
+		printf("}\n");
+
+
+		free(node_data[i].name);
+	}
+
+	free(node_data);
+	return 0;
+}
+
+int cli_addnode(struct backend_conn * conn, int argc, char ** argv) {
+	struct frontend_node_data node_data;
+	int needHelp = 0;
+	
+	if ((needHelp = help_check(argc, argv))) {
+		printf("addnodes adds a node of specified type_id and name to the comp_graph\n");
+		printf("Usage Syntax : \n\t./Cli addnode type_id name\n");
+		printf("Symlink Usage Syntax : \n\t./addnode type_id name\n\n");
+		return 0;
+	}
+
+	if (argc < 3) {
+		printf("Incorrect Usage, run './cli addnode --help' for correct usage.\n");
+		return 1;
+	}
+	
+	node_data.type = atoi(argv[1]);
+	strncpy(node_data.name, argv[2], 512/* this value matches the max space of name */ );
+
+	if (frontend_addnode(conn, &node_data)) {
+		return 1;
+	}
+
+	printf("Node created with block_id : %" PRId16 "\n", node_data.block);
+	return 0;
+}
diff --git a/groundStation/src/cli/cli_nodes.h b/groundStation/src/cli/cli_nodes.h
new file mode 100644
index 0000000000000000000000000000000000000000..2d9bb137272bf0e040feb6bfb420207ba50e0bad
--- /dev/null
+++ b/groundStation/src/cli/cli_nodes.h
@@ -0,0 +1,9 @@
+#ifndef  _CLI_NODES_H
+#define  _CLI_NODES_H
+
+#include "frontend_nodes.h"
+
+int cli_getnodes(struct backend_conn * conn, int argc, char ** argv);
+int cli_addnode(struct backend_conn * conn, int argc, char ** argv);
+
+#endif /* _CLI_NODES_H */
\ No newline at end of file
diff --git a/groundStation/src/cli/cli_output.c b/groundStation/src/cli/cli_output.c
new file mode 100644
index 0000000000000000000000000000000000000000..9f4a5fb5fc90c3209c0c3bb3cd81b497503ca302
--- /dev/null
+++ b/groundStation/src/cli/cli_output.c
@@ -0,0 +1,39 @@
+#include <stdio.h>
+#include <unistd.h>
+#include <inttypes.h>
+
+#include "cli.h"
+#include "cli_output.h"
+#include "frontend_output.h"
+
+int cli_getoutput(struct backend_conn * conn, int argc, char ** argv) {
+	int needHelp = 0;
+	struct frontend_output_data output_data;
+
+	if ((needHelp = help_check(argc, argv))) {
+		printf("getoutput gets the output value of a specified block_id and output_id\n");
+		printf("Usage Syntax : \n\t./Cli getoutput <block_id|'block_name'> <output_id|'output_name'>\n");
+		printf("Symlink Usage Syntax : \n\t./getoutput <block_id|'block_name'> <output_id|'output_name'>\n\n");
+		return 0;
+	}
+
+	if (argc < 3) {
+		printf("Incorrect Usage, run './cli getoutput --help' for correct usage.\n");
+		return 1;
+	}
+
+	if (convert_to_id(conn, argv, (struct convert_data *)&output_data, OUTPUT)) {
+		return 1;
+	}
+
+	if (frontend_getoutput(conn, &output_data)) {
+		return 1;
+	}
+
+	printf("BLOCK.OUTPUT = VAL\n" \
+		   "  %" PRId16 ".%" PRId16 "   =  %lf\n",
+		   output_data.block,
+		   output_data.output,
+		   output_data.value);
+	return 0;
+}
\ No newline at end of file
diff --git a/groundStation/src/cli/cli_output.h b/groundStation/src/cli/cli_output.h
new file mode 100644
index 0000000000000000000000000000000000000000..153dc10bb5d863d8911b6d9ccb931e41ee471caa
--- /dev/null
+++ b/groundStation/src/cli/cli_output.h
@@ -0,0 +1,8 @@
+#ifndef  _CLI_OUTPUT_H
+#define  _CLI_OUTPUT_H
+
+#include "frontend_output.h"
+
+int cli_getoutput(struct backend_conn * conn, int argc, char ** argv);
+
+#endif /* _CLI_OUTPUT_H */
\ No newline at end of file
diff --git a/groundStation/src/cli/cli_param.c b/groundStation/src/cli/cli_param.c
new file mode 100644
index 0000000000000000000000000000000000000000..4b3b802c40760d3b3e4323761f6cd2bb8d4699e4
--- /dev/null
+++ b/groundStation/src/cli/cli_param.c
@@ -0,0 +1,70 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <inttypes.h>
+
+#include "cli.h"
+#include "cli_param.h"
+
+int cli_getparam(struct backend_conn * conn, int argc, char ** argv) {
+	int needHelp = 0;
+	struct frontend_param_data param_data;
+
+
+	if ((needHelp = help_check(argc, argv))) {
+		printf("getparam gets the param_val for a specified block_id and param_id\n");
+		printf("Usage Syntax : \n\t./Cli getparam <block_id|'block_name'> <param_id|'param_name'>\n");
+		printf("Symlink Usage Syntax : \n\t./getparam <block_id|'block_name'> <param_id|'param_name'>\n\n");
+		return 0;
+	}
+
+	if (argc < 3) {
+		printf("Incorrect Usage, run './cli getparam --help' for correct usage.\n");
+		return 1;
+	}
+
+	if (convert_to_id(conn, argv, (struct convert_data *)&param_data, PARAM)) {
+		return 1;
+	}
+	
+	if (frontend_getparam(conn, &param_data)) {
+		return 1;
+	}
+
+	printf("BLOCK.PARAM = VAL\n" \
+		   "   %2" PRId16 ".%2" PRId16 "   =  %lf\n",
+		   param_data.block,
+		   param_data.param,
+		   param_data.value);
+	return 0;
+}
+
+int cli_setparam(struct backend_conn * conn, int argc, char ** argv) {
+	int needHelp = 0;
+	struct frontend_param_data param_data;
+
+	if ((needHelp = help_check(argc, argv))) {
+		printf("setparam sets the param_val for a specified block_id and param_id\n");
+		printf("Usage Syntax : \n\t./Cli setparam <block_id|'block_name'> <param_id|'param_name'> value\n");
+		printf("Symlink Usage Syntax : \n\t./setparam <block_id|'block_name'> <param_id|'param_name'> value\n\n");
+		return 0;
+	}
+
+	if (argc < 4) {
+		printf("Incorrect Usage, run './cli setparam --help' for correct usage.\n");
+		return 1;
+	}
+
+
+	if (convert_to_id(conn, argv, (struct convert_data *)&param_data, PARAM)) {
+		return 1;
+	}
+	
+	param_data.value = atof(argv[3]);
+	
+	if (frontend_setparam(conn, &param_data)) {
+		return 1;
+	}
+	
+	return 0;
+}
diff --git a/groundStation/src/cli/cli_param.h b/groundStation/src/cli/cli_param.h
new file mode 100644
index 0000000000000000000000000000000000000000..b48f033b8fb38c84a062dcd53f15097f2cf4e001
--- /dev/null
+++ b/groundStation/src/cli/cli_param.h
@@ -0,0 +1,14 @@
+#ifndef  _CLI_PARAM_H
+#define  _CLI_PARAM_H
+
+#include <ctype.h>
+#include <string.h>
+
+#include "frontend_param.h"
+#include "frontend_nodes.h"
+#include "graph_blocks.h"
+
+int cli_getparam(struct backend_conn * conn, int argc, char ** argv);
+int cli_setparam(struct backend_conn * conn, int argc, char ** argv);
+
+#endif /* _CLI_PARAM_H */
\ No newline at end of file
diff --git a/groundStation/src/cli/cli_setpid.c b/groundStation/src/cli/cli_setpid.c
deleted file mode 100644
index b2ca124f5e16769dae5d4234d3431ce396210b93..0000000000000000000000000000000000000000
--- a/groundStation/src/cli/cli_setpid.c
+++ /dev/null
@@ -1,103 +0,0 @@
-#include <stdio.h>
-#include <unistd.h>
-#include <getopt.h>
-
-#include "cli_setpid.h"
-#include "frontend_setpid.h"
-
-int cli_setpid(struct backend_conn * conn,	int argc, char **argv) {
-	int c;
-	static int setRoll = 0, setPitch = 0, setYaw = 0, setAll = 0;
-	static int setRollV = 0, setPitchV = 0, setYawV = 0;
-	static int setHeight = 0, setLat = 0, setLong = 0;
-	struct frontend_pid_data pid_data;
-	static int mask;
-	static float pval = 0, ival = 0, dval = 0;
-	static int needHelp = 0;
-	static struct option long_options[] = {
- 		/* These options don’t set a flag. We distinguish them by their indices. */
- 		{"roll",	no_argument,	&setRoll,	1},
- 		{"pitch",   no_argument,   	&setPitch, 	1},
- 		{"yaw",   no_argument,   	&setYaw, 	1},
- 		{"rollv",   no_argument,   	&setRollV, 	1},
- 		{"pitchv",   no_argument,   &setPitchV, 	1},
- 		{"yawv",   no_argument,   	&setYawV, 	1},
- 		{"height",   no_argument,   &setHeight,	1},
- 		{"lat",   no_argument,   	&setLat, 	1},
- 		{"long",   no_argument,   	&setLong, 	1},
- 		{"help",	no_argument,	&needHelp,	1},
- 		{0, 0, 0, 0}
- 	};
-
- 	while (1)
-	{
-		/* getopt_long stores the option index here. */
-		int option_index = 0;
-
-		c = getopt_long(argc, argv, "p:i:d:", long_options, &option_index);
-
-		/* Detect the end of the options. */
-		if (c == -1)
-			break;
-
-		switch(c) {
-			case 'p' :
-				pid_data.p  = atof(optarg);
-				mask |= SET_P;
-				break;
-			case 'i' :
-				pid_data.i  = atof(optarg);
-				mask |= SET_I;
-				break;
-			case 'd' :
-				pid_data.d  = atof(optarg);
-				mask |= SET_D;
-				break;
-			default :
-				break;
-		}
-	}
-
-	if (needHelp) {
-		printf("Setpid sets the p, i , or d constant values of any single controller\n");
-		printf("Usage Syntax : \n\t./Cli setpid controller [options...]\n");
-		printf("Symlink Usage Syntax : \n\t./setpid controller [options...]\n\n");
-		printf("Available 'controllers' include the following\n");
-		printf("\t[--pitch] : Pitch\n\t[--roll] : Roll\n\t[--yaw] : Yaw\n");
-		printf("\t[--pitchv] : Pitch Velocity\n\t[--rollv] : Roll Velocity\n\t[--yawv] : Yaw Velocity\n");
-		printf("\t[--height] : Z\n\t[--lat] : X\n\t[--long] : Y\n\n");
-		printf("Available 'controller' options include the following\n");
-		printf("\t[-p] 'val' : Sets the p constant of the 'controller' to 'val'\n");
-		printf("\t[-i] 'val' : Sets the i constant of the 'controller' to 'val'\n");
-		printf("\t[-d] 'val' : Sets the d constant of the 'controller' to 'val'\n");
-		return 0;
-	}
-
-	if (argc < 2) {
-		printf("Incorrect Usage, run './cli setpid --help' for correct usage.\n");
-		return 1;
-	}
-
-	if (setRoll) {
-		pid_data.controller = PID_ROLL;
-	} else if (setYaw) {
-		pid_data.controller = PID_YAW;
-	} else if (setPitch) {
-		pid_data.controller = PID_PITCH;
-	} else if (setRollV) {
-		pid_data.controller = PID_ROLL_RATE;
-	} else if (setPitchV) {
-		pid_data.controller = PID_PITCH_RATE;
-	} else if (setYawV) {
-		pid_data.controller = PID_YAW_RATE;
-	} else if (setHeight) {
-		pid_data.controller = PID_HEIGHT;
-	} else if (setLong) {
-		pid_data.controller = PID_LONG;
-	} else if (setLat) {
-		pid_data.controller = PID_LAT;
-	}
-
-	frontend_setpid(conn, &pid_data, mask);
-	return 0;
-}
diff --git a/groundStation/src/cli/cli_setpid.h b/groundStation/src/cli/cli_setpid.h
deleted file mode 100644
index d78fcc07042d7d571f35940f798493a5c8cb1c88..0000000000000000000000000000000000000000
--- a/groundStation/src/cli/cli_setpid.h
+++ /dev/null
@@ -1,8 +0,0 @@
-#ifndef __CLI_SETPID_H
-#define __CLI_SETPID_H
-
-#include "frontend_setpid.h"
-
-int cli_setpid(struct backend_conn * conn, int argc, char ** argv);
-
-#endif /* __CLI_SETPID_H */
\ No newline at end of file
diff --git a/groundStation/src/cli/cli_setsetpoint.c b/groundStation/src/cli/cli_setsetpoint.c
deleted file mode 100644
index e0c43814ff98b2e5326b2d6feb0e14fa468d9032..0000000000000000000000000000000000000000
--- a/groundStation/src/cli/cli_setsetpoint.c
+++ /dev/null
@@ -1,89 +0,0 @@
-#include <stdio.h>
-#include <unistd.h>
-#include <getopt.h>
-
-#include "cli_setsetpoint.h"
-
-int cli_setsetpoint(struct backend_conn * conn,	int argc, char **argv) {
-	int c;
-	static int setheight = 0, setlat = 0, setlong = 0;
-	static int setpitch = 0, setroll = 0, setyaw = 0;
-	struct frontend_setpoint_data setpoint_data;
-	static int needHelp = 0;
-	static int mask;
-	static struct option long_options[] = {
- 		/* These options don’t set a flag. We distinguish them by their indices. */
- 		{"help",	no_argument,	    &needHelp,	1},
- 		{"height",  required_argument,	0,  'h'},
- 		{"long",	required_argument,	0,	'l'},
- 		{"lat",		required_argument, 	0,	't'},
- 		{"pitch",   required_argument,	0,  'p'},
- 		{"roll",	required_argument,	0,	'r'},
- 		{"yaw",		required_argument, 	0,	'y'},
- 		{0, 0, 0, 0}
- 	};
-
- 	while (1)
-	{
-		/* getopt_long stores the option index here. */
-		int option_index = 0;
-
-		c = getopt_long(argc, argv, "", long_options, &option_index);
-
-		/* Detect the end of the options. */
-		if (c == -1)
-			break;
-
-		switch(c) {
-			case 'h' :
-				setpoint_data.height  = atof(optarg);
-				mask |= SET_HEIGHT;
-				break;
-			case 't' :
-				setpoint_data.lat  = atof(optarg);
-				mask |= SET_LAT;
-				break;
-			case 'l' :
-				setpoint_data.longg  = atof(optarg);
-				mask |= SET_LONG;
-				break;
-			case 'p' :
-				setpoint_data.pitch  = atof(optarg);
-				mask |= SET_PITCH;
-				break;
-			case 'r' :
-				setpoint_data.roll  = atof(optarg);
-				mask |= SET_ROLL;
-				break;
-			case 'y' :
-				setpoint_data.yaw  = atof(optarg);
-				mask |= SET_YAW;
-				break;
-			default :
-				break;
-		}
-
-	}
-
-	if (needHelp) {
-		printf("Setsetpoint sets the height, lat, long, pitch, roll, or yaw set point values for the quad.\n");
-		printf("Usage Syntax : \n\t./Cli setsetpoint [options...]\n");
-		printf("Symlink Usage Syntax : \n\t./setsetpoint [options...]\n\n");
-		printf("Available options include the following\n");
-		printf("\t[--height] 'val' : Sets the height setpoint value to 'val'\n");
-		printf("\t[--lat]    'val' : Sets the latitude setpoint value to 'val'\n");
-		printf("\t[--long]   'val' : Sets the longitude setpoint value to 'val'\n");
-		printf("\t[--pitch]  'val' : Sets the pitch setpoint value to 'val'\n");
-		printf("\t[--roll]   'val' : Sets the roll setpoint value to 'val'\n");
-		printf("\t[--yaw]    'val' : Sets the yaw setpoint value to 'val'\n");
-		return 0;
-	}
-
-	if (argc < 2) {
-		printf("Incorrect Usage, run './cli setpid --help' for correct usage.\n");
-		return 1;
-	}
-	
-	frontend_setsetpoint(conn, &setpoint_data, mask);
-	return 0;
-}
diff --git a/groundStation/src/cli/cli_setsetpoint.h b/groundStation/src/cli/cli_setsetpoint.h
deleted file mode 100644
index 514145a9485733276fb1982418f5a44747103e74..0000000000000000000000000000000000000000
--- a/groundStation/src/cli/cli_setsetpoint.h
+++ /dev/null
@@ -1,8 +0,0 @@
-#ifndef __CLI_SETSETPOINT_H
-#define __CLI_SETSETPOINT_H
-
-#include "frontend_setsetpoint.h"
-
-int cli_setsetpoint(struct backend_conn * conn, int argc, char ** argv);
-
-#endif /* __CLI_SETSETPOINT_H */
\ No newline at end of file
diff --git a/groundStation/src/cli/cli_source.c b/groundStation/src/cli/cli_source.c
new file mode 100644
index 0000000000000000000000000000000000000000..c82443118163cca8f4ef425a7a912210b6a9bd8a
--- /dev/null
+++ b/groundStation/src/cli/cli_source.c
@@ -0,0 +1,73 @@
+#include <stdio.h>
+#include <unistd.h>
+#include <inttypes.h>
+
+#include "cli.h"
+#include "cli_source.h"
+#include "frontend_source.h"
+
+int cli_getsource(struct backend_conn * conn, int argc, char ** argv) {
+	struct frontend_source_data source_data;
+	int needHelp = 0;
+	
+	if ((needHelp = help_check(argc, argv))) {
+		printf("getsource gets the src_block_id and src_output_id for a specified dst_block_id and dst_input_id\n");
+		printf("Usage Syntax : \n\t./Cli getsource dst_block_id dst_input_id\n");
+		printf("Symlink Usage Syntax : \n\t./getsource dst_block_id dst_input_id\n\n");
+		return 0;
+	}
+
+	if (argc < 3) {
+		printf("Incorrect Usage, run './cli getsource --help' for correct usage.\n");
+		return 1;
+	}
+	
+	if (convert_to_id(conn, argv, (struct convert_data *)&source_data, INPUT)) {
+		return 1;
+	}
+
+	if (frontend_getsource(conn, &source_data)) {
+		return 1;
+	}
+
+	printf("SRC BLOCK.OUTPUT  --->  DST BLOCK.INPUT\n" \
+		   "      %" PRId16 ".%" PRId16 "     --->        %" PRId16 ".%" PRId16 "\n",
+		source_data.src_block, source_data.src_output,
+		source_data.dst_block, source_data.dst_input);
+	return 0;
+}
+
+int cli_setsource(struct backend_conn * conn, int argc, char ** argv) {
+	struct frontend_source_data source_data;
+	struct convert_data conv_data;
+	int needHelp = 0;
+	
+	if ((needHelp = help_check(argc, argv))) {
+		printf("setsource sets the src_block_id and src_output_id for a specified dst_block_id and dst_input_id\n");
+		printf("Usage Syntax : \n\t./Cli setsource dst_block_id dst_input_id src_block_id src_output_id\n");
+		printf("Symlink Usage Syntax : \n\t./setsource dst_block_id dst_input_id src_block_id src_output_id\n\n");
+		return 0;
+	}
+
+	if (argc < 5) {
+		printf("Incorrect Usage, run './cli setsource --help' for correct usage.\n");
+		return 1;
+	}
+	
+	if (convert_to_id(conn, argv, &conv_data, INPUT)) {
+		return 1;
+	}
+	source_data.dst_block = conv_data.val_1;
+	source_data.dst_input = conv_data.val_2;
+
+	if (convert_to_id(conn, argv+2, &conv_data, OUTPUT)) {
+		return 1;
+	}
+	source_data.src_block = conv_data.val_1;
+	source_data.src_output = conv_data.val_2;
+
+	if (frontend_setsource(conn, &source_data)) {
+		return 1;
+	}
+	return 0;
+}
diff --git a/groundStation/src/cli/cli_source.h b/groundStation/src/cli/cli_source.h
new file mode 100644
index 0000000000000000000000000000000000000000..0e1b30cba38db0d7fb2a335aaea89d04cd7c900f
--- /dev/null
+++ b/groundStation/src/cli/cli_source.h
@@ -0,0 +1,9 @@
+#ifndef  _CLI_SOURCE_H
+#define  _CLI_SOURCE_H
+
+#include "frontend_source.h"
+
+int cli_getsource(struct backend_conn * conn, int argc, char ** argv);
+int cli_setsource(struct backend_conn * conn, int argc, char ** argv);
+
+#endif /* _CLI_SOURCE_H */
\ No newline at end of file
diff --git a/groundStation/src/frontend/frontend_common.h b/groundStation/src/frontend/frontend_common.h
index ee0e923bb8e55ab3b1fd48b4da63a7c985ac9845..2fef5c52ae8978e70b153f0579b063d62d979fe4 100644
--- a/groundStation/src/frontend/frontend_common.h
+++ b/groundStation/src/frontend/frontend_common.h
@@ -1,5 +1,5 @@
-#ifndef __FRONTEND_COMMON_H
-#define __FRONTEND_COMMON_H
+#ifndef _FRONTEND_COMMON_H
+#define _FRONTEND_COMMON_H
 #include <stdlib.h>
 
 #ifdef __cplusplus
@@ -23,8 +23,32 @@ char * ucart_backendGetline(struct backend_conn * conn);
 /* Write a line to the backend */
 int ucart_backendWrite(struct backend_conn * backend, const char * line);
 
+struct frontend_output_data {
+	int16_t block;
+	int16_t output;
+	float value;
+};
+
+struct frontend_param_data {
+	int16_t block;
+	int16_t param;
+	float value;
+};
+
+struct frontend_source_data {
+	int16_t dst_block;
+	int16_t dst_input;
+	int16_t src_block;
+	int16_t src_output;
+};
+
+struct frontend_node_data {
+	int16_t block;
+	int16_t type;
+	char *name;
+};
+
 #ifdef __cplusplus
 }
 #endif
-
 #endif /* __FRONTEND_COMMON_H */
diff --git a/groundStation/src/frontend/frontend_getimu.h b/groundStation/src/frontend/frontend_getimu.h
deleted file mode 100644
index 1747252bbc78a403799fb0744c91e2b04883c0cb..0000000000000000000000000000000000000000
--- a/groundStation/src/frontend/frontend_getimu.h
+++ /dev/null
@@ -1,6 +0,0 @@
-#ifndef __FRONTEND_GETIMU_H
-#define __FRONTEND_GETIMU_H
-
-#include "frontend_common.h"
-
-#endif /* __FRONTEND_GETIMU_H */
\ No newline at end of file
diff --git a/groundStation/src/frontend/frontend_getpid.c b/groundStation/src/frontend/frontend_getpid.c
deleted file mode 100644
index 2fd33c31116bf135a3c1f78cf94b2353bdbb8820..0000000000000000000000000000000000000000
--- a/groundStation/src/frontend/frontend_getpid.c
+++ /dev/null
@@ -1,91 +0,0 @@
-#include <stdio.h>
-#include <string.h>
-#include <stdlib.h>
-#include <err.h>
-
-#include "frontend_getpid.h"
-#include "pid_common.h"
-
-/* Get a specified PID.
- *
- * Example:
- *
- * struct frontend_pid_data pid_data;
- * pid_data.pid = PID_PITCH;
- * if (frontend_getpid(conn, &pid_data)) {
- * 		error
- * } else {
- * 		pid_data.p, pid_data.i, and pid_data.d are filled
- * }
- *
- * Returns 0 on success, 1 on error
- */
-int frontend_getpid(
-		struct backend_conn * conn, struct frontend_pid_data * pid_data) {
-	
-	char line[100] = "";
-	switch (pid_data->controller) {
-		case PID_PITCH :
-			strncpy(line, "getpitchp\ngetpitchi\ngetpitchd\n", 30);
-			break;
-		case PID_ROLL :
-			strncpy(line, "getrollp\ngetrolli\ngetrolld\n", 27);
-			break;
-		case PID_YAW :
-			strncpy(line, "getyawp\ngetyawi\ngetyawd\n", 24);
-			break;
-		case PID_PITCH_RATE :
-			strncpy(line, "getpitchratep\ngetpitchratei\ngetpitchrated\n", 42);
-			break;
-		case PID_ROLL_RATE :
-			strncpy(line, "getrollratep\ngetrollratei\ngetrollrated\n", 39);
-			break;
-		case PID_YAW_RATE :
-			strncpy(line, "getyawratep\ngetyawratei\ngetyawrated\n", 36);
-			break;
-		case PID_HEIGHT :
-			strncpy(line, "getheightp\ngetheighti\ngetheightd\n", 33);
-			break;
-		case PID_LAT :
-			strncpy(line, "getlatp\ngetlati\ngetlatd\n", 24);
-			break;
-		case PID_LONG :
-			strncpy(line, "getlongp\ngetlongi\ngetlongd\n", 27);
-			break;
-		default :
-			return 1;
-	}
-
-	int size;
-	if((size = ucart_backendWrite(conn, line)) < 0 ) {
-		return 1;
-	}
-
-	char * response;
-	char tmpresponse[64];
-	char * cmdString;
-	size_t pendingResponses = 3;
-	float value;
-	while (pendingResponses) {
-		response = ucart_backendGetline(conn);
-		if (response == NULL) {
-			warnx("Line not returned from backend");
-			return 1;
-		}
-		strncpy(tmpresponse, response, 64);
-		if (strncmp(tmpresponse, "get", 3) == 0) {
-			cmdString = strtok(tmpresponse, " ");
-			value = strtof(&response[strlen(cmdString)],NULL);
-			
-			if(cmdString[strlen(cmdString)-1] == 'p') {
-				pid_data->p = value;
-			} else if(cmdString[strlen(cmdString)-1] == 'i') {
-				pid_data->i = value;
-			} else if(cmdString[strlen(cmdString)-1] == 'd') {
-				pid_data->d = value;
-			}
-			pendingResponses--;
-		}
-	}
-	return 0;
-}	
diff --git a/groundStation/src/frontend/frontend_getpid.h b/groundStation/src/frontend/frontend_getpid.h
deleted file mode 100644
index e7933f1abf1869ac7f3074d472c8a8a36106507c..0000000000000000000000000000000000000000
--- a/groundStation/src/frontend/frontend_getpid.h
+++ /dev/null
@@ -1,26 +0,0 @@
-#ifndef __FRONTEND_GETPID_H
-#define __FRONTEND_GETPID_H
-
-#include "frontend_common.h"
-#include "pid_common.h"
-
-/* Get a specified PID.
- *
- * Example:
- *
- * struct frontend_pid_data pid_data;
- * pid_data.pid = PITCH;
- * if (frontend_getpid(conn, &pid_data)) {
- * 		error
- * } else {
- * 		pid_data.p, pid_data.i, and pid_data.d are filled
- * }
- *
- * Returns 0 on success, 1 on error
- */
-int frontend_getpid(
-		struct backend_conn * conn,
-		struct frontend_pid_data * pid_data);
-
-
-#endif /* __FRONTEND_GETPID_H */
\ No newline at end of file
diff --git a/groundStation/src/frontend/frontend_getsetpoint.c b/groundStation/src/frontend/frontend_getsetpoint.c
deleted file mode 100644
index b23a5dda43847e08eb3d7eb06479bff27c002575..0000000000000000000000000000000000000000
--- a/groundStation/src/frontend/frontend_getsetpoint.c
+++ /dev/null
@@ -1,94 +0,0 @@
-#include <stdio.h>
-#include <string.h>
-#include <stdlib.h>
-#include <err.h>
-
-#include "frontend_getsetpoint.h"
-#include "setpoint_common.h"
-#include "cli_getsetpoint.h"
-
-/* Get a specified setpoint.
- *
- * Example:
- *
- * struct frontend_setpoint_data setpoint_data;
- * if (frontend_getpid(conn, &pid_data)) {
- * 		error
- * } else {
- * 		setpoint_data.height, setpoint_data.lat, and setpoint.long are filled
- * }
- *
- * Returns 0 on success, 1 on error
- */
-int frontend_getsetpoint(
-		struct backend_conn * conn, struct frontend_setpoint_data * setpoint_data, int type) {
-	
-	char line[25] = "";
-	switch (type) {
-		case HEIGHT :
-			strncpy(line, "getheight\n", 10);
-			break;
-		case LAT :
-			strncpy(line, "getlat\n", 7);
-			break;
-		case LONGG :
-			strncpy(line, "getlong\n", 8);
-			break;
-		case PITCH :
-			strncpy(line, "getpitch\n", 9);
-			break;
-		case ROLL :
-			strncpy(line, "getroll\n", 8);
-			break;
-		case YAW :
-			strncpy(line, "getyaw\n", 7);
-			break;
-	}
-	int size;
-	if((size = ucart_backendWrite(conn, line)) < 0 ) {
-		return 1;
-	}
-
-	char * response;
-	char tmpresponse[64];
-	char * cmdString;
-	size_t pendingResponses = 1;
-	float value;
-	while (pendingResponses) {
-		response = ucart_backendGetline(conn);
-		if (response == NULL) {
-			warnx("Line not returned from backend");
-			return 1;
-		}
-		strncpy(tmpresponse, response, 64);
-		if (strncmp(tmpresponse, "get", 3) == 0) {
-			cmdString = strtok(tmpresponse, " ");
-			value = strtof(&response[strlen(cmdString)],NULL);
-			
-			switch (type) {
-				case HEIGHT :
-					setpoint_data->height = value;
-					break;
-				case LAT :
-					setpoint_data->lat = value;
-					break;
-				case LONGG :
-					setpoint_data->longg = value;
-					break;
-				case PITCH :
-					setpoint_data->pitch = value;
-					break;
-				case ROLL :
-					setpoint_data->roll = value;
-					break;
-				case YAW :
-					setpoint_data->yaw = value;
-					break;
-			}
-
-			pendingResponses--;
-		}
-	}
-
-	return 0;
-}	
diff --git a/groundStation/src/frontend/frontend_getsetpoint.h b/groundStation/src/frontend/frontend_getsetpoint.h
deleted file mode 100644
index f8dfdc87b3df057d5fd43714c5b81a52831ce763..0000000000000000000000000000000000000000
--- a/groundStation/src/frontend/frontend_getsetpoint.h
+++ /dev/null
@@ -1,25 +0,0 @@
-#ifndef __FRONTEND_GETSETPOINT_H
-#define __FRONTEND_GETSETPOINT_H
-
-#include "frontend_common.h"
-#include "setpoint_common.h"
-
-/* Get a specified setpoint.
- *
- * Example:
- *
- * struct frontend_setpoint_data setpoint_data;
- * if (frontend_getpid(conn, &pid_data)) {
- * 		error
- * } else {
- * 		setpoint_data.height, setpoint_data.lat, and setpoint.long are filled
- * }
- *
- * Returns 0 on success, 1 on error
- */
-int frontend_getsetpoint(
-		struct backend_conn * conn,
-		struct frontend_setpoint_data * setpoint_data,
-		int type);
-
-#endif /* __FRONTEND_GETPID_H */
\ No newline at end of file
diff --git a/groundStation/src/frontend/frontend_nodes.c b/groundStation/src/frontend/frontend_nodes.c
new file mode 100644
index 0000000000000000000000000000000000000000..6f2ef72e9c6bedda79d88be8ce9f997730fa35e8
--- /dev/null
+++ b/groundStation/src/frontend/frontend_nodes.c
@@ -0,0 +1,125 @@
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <err.h>
+#include <inttypes.h>
+
+#include "frontend_nodes.h"
+
+/* Get the block_id, type_id and name of
+ * all of the nodes in the current comp_graph.
+ *
+ * Returns 0 on success, 1 on error
+ *
+ * 
+ * Note : node_data may be resized inside this function. 
+ * That is why you must pass in a strcut ... **
+ */
+int frontend_getnodes(
+		struct backend_conn * conn,
+		struct frontend_node_data ** node_data,
+		size_t * num_nodes) {
+
+	char msg[64] = "";
+	int written;
+
+	snprintf(msg, 64, "getnodes\n");
+
+	if((written = ucart_backendWrite(conn, msg)) < 0) {
+		return 1;
+	}
+
+	size_t pendingResponses = 1;
+	char * response;
+
+	while (pendingResponses) {
+		response = ucart_backendGetline(conn);
+		if (response == NULL) {
+			warnx("Line not returned from backend");
+			return 1;
+		}
+
+		if (strncmp(response, "getnodes", 8) != 0) {
+			continue;
+		}
+
+		size_t num_nodes_ret = 0;
+		size_t offset = 0;
+
+		sscanf(response, "getnodes %lu %n", &num_nodes_ret,  (int * )&offset);
+		response += offset;
+
+		/* Resize if necessary */
+		if (num_nodes_ret != *num_nodes) {
+			*num_nodes = num_nodes_ret;
+			*node_data = realloc(*node_data, sizeof(**node_data) * (*num_nodes));
+			if (!*node_data) {
+				return 1;
+			}
+		}
+
+		struct frontend_node_data * arr = *node_data;
+		for (size_t i = 0; i < *num_nodes; ++i) {
+			sscanf(response, "%" SCNd16 " %n", &arr[i].block, (int*)&offset);
+			// printf("found block %d\n", arr[i].block);
+			response += offset;
+		}
+
+		for (size_t i = 0; i < *num_nodes; ++i) {
+			sscanf(response, "%" SCNd16 " %n", &arr[i].type, (int*)&offset);
+			// printf("found type %d\n", arr[i].type);
+			response += offset;
+		}
+
+		for (size_t i = 0; i < *num_nodes; ++i) {
+			char tmp_name[512];
+			sscanf(response, " '%[^\t\n']' %n", (char *)&tmp_name, (int *)&offset);
+			arr[i].name = malloc(sizeof(*arr[i].name) * strlen(tmp_name) + 1);
+			strcpy((char*)arr[i].name, tmp_name);
+			// printf("found name '%s'\n", arr[i].name);
+			response += offset;
+		}
+		pendingResponses--;
+	}
+	
+	return 0;
+}
+
+/* Set the value of block.param
+ *
+ * Returns 0 on success, 1 on error
+ */
+int frontend_addnode(
+		struct backend_conn * conn,
+		struct frontend_node_data * node_data) {
+	
+	char msg[16 + strlen(node_data->name)];
+	int written;
+
+	snprintf(msg, 16 + strlen(node_data->name), "addnode %" PRId16 " %s\n", node_data->type, node_data->name);
+
+	if((written = ucart_backendWrite(conn, msg)) < 0) {
+		return 1;
+	}
+
+	size_t pendingResponses = 1;
+	char * response;
+
+	while (pendingResponses) {
+		response = ucart_backendGetline(conn);
+		if (response == NULL) {
+			warnx("Line not returned from backend");
+			return 1;
+		}
+
+		if (strncmp(response, "addnode", 7) != 0) {
+			continue;
+		}
+
+		sscanf(response, "addnode %" SCNd16, &node_data->block);
+
+		pendingResponses--;
+	}
+	
+	return 0;
+}
\ No newline at end of file
diff --git a/groundStation/src/frontend/frontend_nodes.h b/groundStation/src/frontend/frontend_nodes.h
new file mode 100644
index 0000000000000000000000000000000000000000..8410d12decb915d23e77e9c71263065177ecb58b
--- /dev/null
+++ b/groundStation/src/frontend/frontend_nodes.h
@@ -0,0 +1,28 @@
+#ifndef _FRONTEND_NODES_H
+#define _FRONTEND_NODES_H
+
+#include "frontend_common.h"
+
+/* Get the block_id, type_id and name of
+ * all of the nodes in the current comp_graph.
+ *
+ * Returns 0 on success, 1 on error
+ *
+ * Note : node_data may be resized inside this function. 
+ * That is why you must pass in a strcut ... **
+ */
+int frontend_getnodes(
+		struct backend_conn * conn,
+		struct frontend_node_data ** node_data,
+		size_t * num_nodes);
+
+
+/* Adds a node of specified type_id
+ *
+ * Returns 0 on success, 1 on error
+ */
+int frontend_addnode(
+		struct backend_conn * conn,
+		struct frontend_node_data * node_data);
+
+#endif /* __FRONTEND_NODES_H */
\ No newline at end of file
diff --git a/groundStation/src/frontend/frontend_output.c b/groundStation/src/frontend/frontend_output.c
new file mode 100644
index 0000000000000000000000000000000000000000..54cd6cf8873605c1c166e95d7b7992dcb3810efa
--- /dev/null
+++ b/groundStation/src/frontend/frontend_output.c
@@ -0,0 +1,46 @@
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <err.h>
+#include <inttypes.h>
+
+#include "frontend_output.h"
+
+/* Get the value that corresponds 
+ *      to the block.output in question.
+ *
+ * Returns 0 on success, 1 on error
+ */
+int frontend_getoutput(
+		struct backend_conn * conn,
+		struct frontend_output_data * output_data) {
+
+	char msg[64] = "";
+	int written;
+
+	snprintf(msg, 64, "getoutput %" PRId16 " %" PRId16 "\n", output_data->block, output_data->output);
+
+	if((written = ucart_backendWrite(conn, msg)) < 0) {
+		return 1;
+	}
+
+	size_t pendingResponses = 1;
+	char * response;
+
+	while (pendingResponses) {
+		response = ucart_backendGetline(conn);
+		if (response == NULL) {
+			warnx("Line not returned from backend");
+			return 1;
+		}
+
+		if (strncmp(response, "getoutput", 9) == 0) {
+			sscanf(response, "getoutput %" SCNd16 " %" SCNd16" %f", 
+				&output_data->block, &output_data->output,
+				&output_data->value);
+			pendingResponses--;
+		}
+	}
+	
+	return 0;
+}
\ No newline at end of file
diff --git a/groundStation/src/frontend/frontend_output.h b/groundStation/src/frontend/frontend_output.h
new file mode 100644
index 0000000000000000000000000000000000000000..b2a4e4333cc83344c0154c9bc20d764d9439799d
--- /dev/null
+++ b/groundStation/src/frontend/frontend_output.h
@@ -0,0 +1,16 @@
+#ifndef _FRONTEND_OUTPUT_H
+#define _FRONTEND_OUTPUT_H
+
+#include "frontend_common.h"
+
+/* Get the value that corresponds 
+ *      to the block.output in question.
+ *
+ * Returns 0 on success, 1 on error
+ */
+int frontend_getoutput(
+		struct backend_conn * conn,
+		struct frontend_output_data * output_data);
+
+
+#endif /* __FRONTEND_OUTPUT_H */
\ No newline at end of file
diff --git a/groundStation/src/frontend/frontend_param.c b/groundStation/src/frontend/frontend_param.c
new file mode 100644
index 0000000000000000000000000000000000000000..d16faa64474a0ac199becb88c3453f766577b02a
--- /dev/null
+++ b/groundStation/src/frontend/frontend_param.c
@@ -0,0 +1,66 @@
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <err.h>
+#include <inttypes.h>
+
+#include "frontend_param.h"
+
+
+/* Get the value of block.param
+ *
+ * Returns 0 on success, 1 on error
+ */
+int frontend_getparam(
+		struct backend_conn * conn,
+		struct frontend_param_data * param_data) {
+
+	char msg[64] = "";
+	int written;
+
+	snprintf(msg, 64, "getparam %" PRId16 " %" PRId16 "\n", param_data->block, param_data->param);
+
+	if((written = ucart_backendWrite(conn, msg)) < 0) {
+		return 1;
+	}
+
+	size_t pendingResponses = 1;
+	char * response;
+
+	while (pendingResponses) {
+		response = ucart_backendGetline(conn);
+		if (response == NULL) {
+			warnx("Line not returned from backend");
+			return 1;
+		}
+
+		if (strncmp(response, "getparam", 8) == 0) {
+			sscanf(response, "getparam %" SCNd16 " %" SCNd16 " %f", 
+				&param_data->block, &param_data->param,
+				&param_data->value);
+			pendingResponses--;
+		}
+	}
+	
+	return 0;
+}
+
+/* Set the value of block.param
+ *
+ * Returns 0 on success, 1 on error
+ */
+int frontend_setparam(
+		struct backend_conn * conn,
+		struct frontend_param_data * param_data) {
+
+	char msg[64] = "";
+	int written;
+
+	snprintf(msg, 64, "setparam %" PRId16 " %" PRId16 " %f\n", param_data->block, param_data->param, param_data->value);
+
+	if((written = ucart_backendWrite(conn, msg)) < 0) {
+		return 1;
+	}
+	
+	return 0;
+}
\ No newline at end of file
diff --git a/groundStation/src/frontend/frontend_param.h b/groundStation/src/frontend/frontend_param.h
new file mode 100644
index 0000000000000000000000000000000000000000..26854dd0d7ee104e20edb524a5459a2535794426
--- /dev/null
+++ b/groundStation/src/frontend/frontend_param.h
@@ -0,0 +1,23 @@
+#ifndef _FRONTEND_PARAM_H
+#define _FRONTEND_PARAM_H
+
+#include "frontend_common.h"
+
+/* Get the value of block.param
+ *
+ * Returns 0 on success, 1 on error
+ */
+int frontend_getparam(
+		struct backend_conn * conn,
+		struct frontend_param_data * param_data);
+
+/* Set the value of block.param
+ *
+ * Returns 0 on success, 1 on error
+ */
+int frontend_setparam(
+		struct backend_conn * conn,
+		struct frontend_param_data * param_data);
+
+
+#endif /* __FRONTEND_PARAM_H */
\ No newline at end of file
diff --git a/groundStation/src/frontend/frontend_setpid.c b/groundStation/src/frontend/frontend_setpid.c
deleted file mode 100644
index 58bf4109cd1c50d22486e464c101c9f8dd3c367e..0000000000000000000000000000000000000000
--- a/groundStation/src/frontend/frontend_setpid.c
+++ /dev/null
@@ -1,82 +0,0 @@
-#include <err.h>
-#include <stdio.h>
-
-#include "frontend_setpid.h"
-#include "pid_common.h"
-#include "frontend_common.h"
-
-int frontend_setpid(
-		struct backend_conn * conn,
-		struct frontend_pid_data * pid_data,
-		int mask)
-{
-	if (conn == NULL) {
-		return -1;
-	}
-
-	char * controller;
-	switch (pid_data->controller) {
-		case PID_PITCH:
-			controller = "pitch";
-			break;
-		case PID_ROLL:
-			controller = "roll";
-			break;
-		case PID_YAW:
-			controller = "yaw";
-			break;
-		case PID_PITCH_RATE:
-			controller = "pitchrate";
-			break;
-		case PID_ROLL_RATE:
-			controller = "rollrate";
-			break;
-		case PID_YAW_RATE:
-			controller = "yawrate";
-			break;
-		case PID_HEIGHT:
-			controller = "height";
-			break;
-		case PID_LAT:
-			controller = "lat";
-			break;
-		case PID_LONG:
-			controller = "long";
-			break;
-		default:
-			warnx("Unsupported PID constant");
-			return -1;
-	}
-
-	char buffer[2048];
-	/* Set the P, I, and D */
-	if (mask & SET_P) {
-		if (snprintf(buffer, 2048,
-					"set%sp %f\n", 
-					controller, 
-					pid_data->p) >= 2048) {
-			errx(0, "Buffer to small to format!");
-		}
-		ucart_backendWrite(conn, buffer);
-	}
-	if (mask & SET_I) {
-		if (snprintf(buffer, 2048,
-					"set%si %f\n", 
-					controller, 
-					pid_data->i) >= 2048) {
-			errx(0, "Buffer to small to format!");
-		}
-		ucart_backendWrite(conn, buffer);
-	}
-	if (mask & SET_D) {
-		if (snprintf(buffer, 2048,
-					"set%sd %f\n", 
-					controller, 
-					pid_data->d) >= 2048) {
-			errx(0, "Buffer to small to format!");
-		}
-		ucart_backendWrite(conn, buffer);
-	}
-
-	return 0;
-}
diff --git a/groundStation/src/frontend/frontend_setpid.h b/groundStation/src/frontend/frontend_setpid.h
deleted file mode 100644
index b9c6e073d3433a0350ebd9b2d9de8ee55a12f18e..0000000000000000000000000000000000000000
--- a/groundStation/src/frontend/frontend_setpid.h
+++ /dev/null
@@ -1,20 +0,0 @@
-#ifndef __FRONTEND_SETPID_H
-#define __FRONTEND_SETPID_H
-
-#include "pid_common.h"
-#include "frontend_common.h"
-
-int frontend_setpid(
-		struct backend_conn * conn,
-		struct frontend_pid_data * pid_data,
-		int mask);
-
-#define SET_P 0x01
-#define SET_I 0x02
-#define SET_D 0x04
-
-#ifndef SET_ALL
-#define SET_ALL (SET_P | SET_I | SET_D)
-#endif /* SET_ALL */
-
-#endif /* __FRONTEND_SETPID_H */
\ No newline at end of file
diff --git a/groundStation/src/frontend/frontend_setsetpoint.c b/groundStation/src/frontend/frontend_setsetpoint.c
deleted file mode 100644
index b9f43caed0ded4d80964dddcc39a12e165f5c54a..0000000000000000000000000000000000000000
--- a/groundStation/src/frontend/frontend_setsetpoint.c
+++ /dev/null
@@ -1,69 +0,0 @@
-#include <err.h>
-#include <stdio.h>
-
-#include "frontend_setsetpoint.h"
-#include "setpoint_common.h"
-#include "frontend_common.h"
-
-int frontend_setsetpoint(
-		struct backend_conn * conn,
-		struct frontend_setpoint_data * setpoint_data,
-		int mask)
-{
-	if (conn == NULL) {
-		return -1;
-	}
-
-	char buffer[2048];
-	/* Set the P, I, and D */
-	if (mask & SET_HEIGHT) {
-		if (snprintf(buffer, 2048,
-					"setheight %f\n",  
-					setpoint_data->height) >= 2048) {
-			errx(0, "Buffer to small to format!");
-		}
-		ucart_backendWrite(conn, buffer);
-	}
-	if (mask & SET_LAT) {
-		if (snprintf(buffer, 2048,
-					"setlat %f\n",
-					setpoint_data->lat) >= 2048) {
-			errx(0, "Buffer to small to format!");
-		}
-		ucart_backendWrite(conn, buffer);
-	}
-	if (mask & SET_LONG) {
-		if (snprintf(buffer, 2048,
-					"setlong %f\n",
-					setpoint_data->longg) >= 2048) {
-			errx(0, "Buffer to small to format!");
-		}
-		ucart_backendWrite(conn, buffer);
-	}
-	if (mask & SET_PITCH) {
-		if (snprintf(buffer, 2048,
-					"setpitch %f\n",  
-					setpoint_data->pitch) >= 2048) {
-			errx(0, "Buffer to small to format!");
-		}
-		ucart_backendWrite(conn, buffer);
-	}
-	if (mask & SET_ROLL) {
-		if (snprintf(buffer, 2048,
-					"setroll %f\n",
-					setpoint_data->roll) >= 2048) {
-			errx(0, "Buffer to small to format!");
-		}
-		ucart_backendWrite(conn, buffer);
-	}
-	if (mask & SET_YAW) {
-		if (snprintf(buffer, 2048,
-					"setyaw %f\n",
-					setpoint_data->yaw) >= 2048) {
-			errx(0, "Buffer to small to format!");
-		}
-		ucart_backendWrite(conn, buffer);
-	}
-
-	return 0;
-}
diff --git a/groundStation/src/frontend/frontend_setsetpoint.h b/groundStation/src/frontend/frontend_setsetpoint.h
deleted file mode 100644
index 2607c4d564e14b5c4baae8b9500a74d6b878b922..0000000000000000000000000000000000000000
--- a/groundStation/src/frontend/frontend_setsetpoint.h
+++ /dev/null
@@ -1,23 +0,0 @@
-#ifndef __FRONTEND_SETSETPOINT_H
-#define __FRONTEND_SETSETPOINT_H
-
-#include "setpoint_common.h"
-#include "frontend_common.h"
-
-int frontend_setsetpoint(
-		struct backend_conn * conn,
-		struct frontend_setpoint_data * setpoint_data,
-		int mask);
-
-#define SET_LAT 0x01
-#define SET_LONG 0x02
-#define SET_HEIGHT 0x04
-#define SET_PITCH 0x08
-#define SET_ROLL  0x10
-#define SET_YAW   0x20
-
-#ifndef SET_ALL
-#define SET_ALL (SET_LAT | SET_LONG | SET_HEIGHT | SET_PITCH | SET_ROLL | SET_YAW)
-#endif /* SET_ALL */
-
-#endif /* __FRONTEND_SETSETPOINT_H */
\ No newline at end of file
diff --git a/groundStation/src/frontend/frontend_source.c b/groundStation/src/frontend/frontend_source.c
new file mode 100644
index 0000000000000000000000000000000000000000..809696fc0d99357ce42440e56620334455fc564f
--- /dev/null
+++ b/groundStation/src/frontend/frontend_source.c
@@ -0,0 +1,69 @@
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <err.h>
+#include <inttypes.h>
+
+#include "frontend_source.h"
+
+
+/* Get the block.output that corresponds 
+ *      to the block.input in question.
+ *
+ * Returns 0 on success, 1 on error
+ */
+int frontend_getsource(
+		struct backend_conn * conn,
+		struct frontend_source_data * source_data) {
+
+	char msg[64] = "";
+	int written;
+
+	snprintf(msg, 64, "getsource %" PRId16 " %" PRId16 "\n", source_data->dst_block, source_data->dst_input);
+
+	if((written = ucart_backendWrite(conn, msg)) < 0) {
+		return 1;
+	}
+
+	size_t pendingResponses = 1;
+	char * response;
+
+	while (pendingResponses) {
+		response = ucart_backendGetline(conn);
+		if (response == NULL) {
+			warnx("Line not returned from backend");
+			return 1;
+		}
+
+		if (strncmp(response, "getsource", 9) == 0) {
+			sscanf(response, "getsource %" SCNd16 " %" SCNd16 " %" SCNd16 " %" SCNd16, 
+				&source_data->dst_block, &source_data->dst_input,
+				&source_data->src_block, &source_data->src_output);
+			pendingResponses--;
+		}
+	}
+	
+	return 0;
+}
+
+/* Set the connect the block.output from one block to 
+ *       another block.input.
+ *
+ * Returns 0 on success, 1 on error
+ */
+int frontend_setsource(
+		struct backend_conn * conn,
+		struct frontend_source_data * source_data) {
+	char msg[64] = "";
+	int written;
+
+	snprintf(msg, 64, "setsource %" PRId16 " %" PRId16 " %" PRId16 " %" PRId16 "\n", 
+		source_data->dst_block, source_data->dst_input, 
+		source_data->src_block, source_data->src_output);
+
+	if((written = ucart_backendWrite(conn, msg)) < 0) {
+		return 1;
+	}
+	
+	return 0;
+}
diff --git a/groundStation/src/frontend/frontend_source.h b/groundStation/src/frontend/frontend_source.h
new file mode 100644
index 0000000000000000000000000000000000000000..ac6da68956e933717a51ee9783bd1a821548a5a0
--- /dev/null
+++ b/groundStation/src/frontend/frontend_source.h
@@ -0,0 +1,25 @@
+#ifndef _FRONTEND_SOURCE_H
+#define _FRONTEND_SOURCE_H
+
+#include "frontend_common.h"
+
+/* Get the block.output that corresponds 
+ *      to the block.input in question.
+ *
+ * Returns 0 on success, 1 on error
+ */
+int frontend_getsource(
+		struct backend_conn * conn,
+		struct frontend_source_data * source_data);
+
+/* Set the connect the block.output from one block to 
+ *       another block.input.
+ *
+ * Returns 0 on success, 1 on error
+ */
+int frontend_setsource(
+		struct backend_conn * conn,
+		struct frontend_source_data * source_data);
+
+
+#endif /* _FRONTEND_SOURCE_H */
\ No newline at end of file
diff --git a/groundStation/src/frontend/pid_common.h b/groundStation/src/frontend/pid_common.h
deleted file mode 100644
index b89e3b1cddc85bb8820520e91ea4fa1858becd3a..0000000000000000000000000000000000000000
--- a/groundStation/src/frontend/pid_common.h
+++ /dev/null
@@ -1,26 +0,0 @@
-#ifndef __PID_COMMON_H
-#define __PID_COMMON_H
-
-enum pid_controller {
-	PID_PITCH,
-	PID_ROLL,
-	PID_YAW,
-	PID_PITCH_RATE,
-	PID_ROLL_RATE,
-	PID_YAW_RATE,
-	PID_HEIGHT, /* Z */
-	PID_LAT, /* X */
-	PID_LONG, /* Y */
-	PID_NUM_PIDS
-};
-
-struct frontend_pid_data {
-	enum pid_controller controller;
-
-	float p;
-	float i;
-	float d;
-};
-
-
-#endif /* __PID_COMMON_H */
diff --git a/groundStation/src/frontend/setpoint_common.h b/groundStation/src/frontend/setpoint_common.h
deleted file mode 100644
index a661cb1cd6b2a3fe84edbba7fdf2ffac85e9c074..0000000000000000000000000000000000000000
--- a/groundStation/src/frontend/setpoint_common.h
+++ /dev/null
@@ -1,15 +0,0 @@
-#ifndef __SETPOINT_COMMON_H
-#define __SETPOINT_COMMON_H
-
-struct frontend_setpoint_data {
-	float height;
-	float lat;
-	/* named with two g's due to long type name */
-	float longg;
-	float pitch;
-	float roll;
-	float yaw;
-};
-
-
-#endif /* __SETPOINT_COMMON_H */
\ No newline at end of file
diff --git a/quad/.gitignore b/quad/.gitignore
index 43d04fb118c3ab8c1ea06f84c8a8b11598990756..92bd47f407dc5d75d40fff7c329cb9ba87f3df11 100644
--- a/quad/.gitignore
+++ b/quad/.gitignore
@@ -5,3 +5,4 @@ lib/
 lib-zybo/
 TAGS
 out/
+bin/
\ No newline at end of file
diff --git a/quad/Makefile b/quad/Makefile
index f784e354f4310db4d4c9241d11b731c65cba1249..d04ffe0e3d8af5a9842809be690f7ab85d30e134 100644
--- a/quad/Makefile
+++ b/quad/Makefile
@@ -1,20 +1,26 @@
 INCDIR = inc
 LIBDIR = lib
 OUTDIR = out
+EXEDIR = bin
 WS = $(CURDIR)/xsdk_workspace
 
 BOOT = $(OUTDIR)/BOOT.bin
 
 .PHONY: all libs zybo boot test clean deep-clean
 
-all: libs
+all: libs bins
 
 libs:
 	$(MAKE) -C src/test
 	$(MAKE) -C src/queue
 	$(MAKE) -C src/computation_graph
+	$(MAKE) -C src/graph_blocks
+	$(MAKE) -C src/commands
 	$(MAKE) -C src/quad_app
 
+bins:
+	$(MAKE) -C src/virt_quad
+
 zybo:
 	bash scripts/build_zybo.sh
 
@@ -26,13 +32,15 @@ test:
 	$(MAKE) -C src/quad_app test
 
 clean:
-	rm -rf $(INCDIR) $(LIBDIR) $(OUTDIR)
+	rm -rf $(INCDIR) $(LIBDIR) $(OUTDIR) $(EXEDIR)
 
 deep-clean:
 	make clean
 	$(MAKE) -C src/test clean
 	$(MAKE) -C src/queue clean
 	$(MAKE) -C src/computation_graph clean
+	$(MAKE) -C src/graph_blocks clean
+	$(MAKE) -C src/commands clean
 	$(MAKE) -C src/quad_app clean
 	bash scripts/clean_xsdk_workspace.sh
 
diff --git a/quad/executable.mk b/quad/executable.mk
new file mode 100644
index 0000000000000000000000000000000000000000..d17244a837c5c785fcf53ffb3f547d110d0be88a
--- /dev/null
+++ b/quad/executable.mk
@@ -0,0 +1,43 @@
+GCC = gcc
+
+INCDIR = $(TOP)/inc
+OBJDIR = obj
+EXEDIR = $(TOP)/bin
+LIBDIR = $(TOP)/lib
+
+SOURCES = $(wildcard *.c)
+HEADERS = $(wildcard *.h)
+INCLUDES = $(addprefix $(INCDIR)/, $(HEADERS))
+OBJECTS = $(patsubst %.c, $(OBJDIR)/%.o, $(SOURCES))
+
+TARGET = $(EXEDIR)/$(NAME)
+
+.PHONY: default run clean
+
+################
+## User Targets
+################
+
+default: $(TARGET)
+
+run: $(TARGET)
+	$(EXEDIR)/$(NAME)
+
+clean:
+	rm -rf $(TARGET) $(OBJDIR)
+
+####################
+## Internal Targets
+####################
+
+$(TARGET): $(OBJECTS) | $(EXEDIR)
+	$(GCC) -g -o $(TARGET) $^ -I$(INCDIR) -L$(LIBDIR) $(REQLIBS)
+
+$(OBJDIR)/%.o : %.c | $(OBJDIR) $(INCDIR)
+	$(GCC) -c -g -o $@ $< -I$(INCDIR)
+
+$(OBJDIR):
+	mkdir $(OBJDIR)
+
+$(EXEDIR):
+	mkdir $(EXEDIR)
diff --git a/quad/scripts/__pycache__/uart_stress_tests.cpython-36.pyc b/quad/scripts/__pycache__/uart_stress_tests.cpython-36.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..4b92a40fddc351f17e205b35da850c41744a373c
Binary files /dev/null and b/quad/scripts/__pycache__/uart_stress_tests.cpython-36.pyc differ
diff --git a/quad/scripts/test_uart_comm.py b/quad/scripts/test_uart_comm.py
index bd4f36c0c9748300850eaf495dc4efdf3a7d3926..c02b017aaf3079ac1222ea71e67b9c8d2c2ac220 100755
--- a/quad/scripts/test_uart_comm.py
+++ b/quad/scripts/test_uart_comm.py
@@ -5,11 +5,10 @@ import time
 
 import serial
 
-def create_msg(main_type, subtype, msg_id, data):
+def create_msg(msg_type, msg_id, data):
     msg = bytes()
     msg += b'\xBE'
-    msg += main_type.to_bytes(1, 'little')
-    msg += subtype.to_bytes(1, 'little')
+    msg += msg_type.to_bytes(2, 'little')
     msg += msg_id.to_bytes(2, 'little')
     msg += len(data).to_bytes(2, 'little')
     msg += data
@@ -22,7 +21,7 @@ def create_msg(main_type, subtype, msg_id, data):
 
 def create_test_packet(size=8):
     data = bytes((i % 256 for i in range(size)))
-    return create_msg(0, 1, 0, data)
+    return create_msg(1, 0, data)
 
 def read_packet(ser):
     header = ser.read(7)
@@ -33,7 +32,7 @@ def read_packet(ser):
 
 def query_received(ser):
     # Send request
-    query_msg = create_msg(0, 2, 0, b'')
+    query_msg = create_msg(2, 0, b'')
     ser.write(query_msg)
     ser.flush()
     
@@ -46,7 +45,7 @@ def query_received(ser):
 if __name__ == '__main__':
     with serial.Serial('/dev/ttyUSB0', 921600, timeout=5) as ser:
         ser.reset_input_buffer()
-        ser.write(create_test_packet(8))
+        ser.write(create_test_packet(240))
         ser.flush()
         print(query_received(ser))
 
diff --git a/quad/scripts/test_zybo_uart.py b/quad/scripts/test_zybo_uart.py
index 0ccd93c66f47df11e9ad87651e1299ca56f8453c..4b0bfb86775e5c19f99fccc5069208aa304a040c 100755
--- a/quad/scripts/test_zybo_uart.py
+++ b/quad/scripts/test_zybo_uart.py
@@ -5,10 +5,11 @@ import time
 
 import serial
 
-if __name__ == '__main__':
+def do_test():
     with serial.Serial('/dev/ttyUSB0', 921600, timeout=5) as ser:
         ser.reset_input_buffer()
-        send_bytes = b'sdflouirgaorifa;eofija;ogijasfhluiasflawieufzxcvwe'
+        send_bytes = bytes(i % 256 for i in range(128))
+        #send_bytes = b'sdjjjjjjjjjj9ssssssssssssnnnnnnnnnflouirgaorifa;eofija;ogijasfhluiasflawieufzxcvwe'
         ser.write(send_bytes)
         print("Sending {} bytes".format(len(send_bytes)))
         time.sleep(1)
@@ -21,4 +22,8 @@ if __name__ == '__main__':
         else:
             print(recv_bytes);
             print("Test Failed.")
-
+if __name__ == '__main__':
+    for i in range(1):
+        print("Test ", i)
+        do_test()
+        time.sleep(1)
diff --git a/quad/src/commands/Makefile b/quad/src/commands/Makefile
new file mode 100644
index 0000000000000000000000000000000000000000..8cc8ffee3eb119613ad65707e4bcc03f9dea80a1
--- /dev/null
+++ b/quad/src/commands/Makefile
@@ -0,0 +1,6 @@
+TOP=../..
+
+NAME = commands
+REQLIBS = -ltest
+
+include $(TOP)/library.mk
diff --git a/quad/src/quad_app/cb_default.h b/quad/src/commands/cb_default.h
similarity index 75%
rename from quad/src/quad_app/cb_default.h
rename to quad/src/commands/cb_default.h
index aa308108b499e2cb8f4e723a59b81b02abe6d29c..5581c442c29af074806052203d4e18e76d263d5c 100644
--- a/quad/src/quad_app/cb_default.h
+++ b/quad/src/commands/cb_default.h
@@ -1,11 +1,10 @@
 #include "commands.h"
-#include "type_def.h"
 
 /* The cb_default used on the groundStation. This file MUST NOT BE INCLUDED
  * by anything except for commands.c */
 
 /* cb_default used by portable commands.c */
-int cb_default(modular_structs_t *structs)
+int cb_default(struct modular_structs *structs)
 {
 	    return 0;
 }
diff --git a/common/commands.c b/quad/src/commands/commands.c
similarity index 94%
rename from common/commands.c
rename to quad/src/commands/commands.c
index 09f18bcaeefb6c8ecbcffd4342c0ce7b68c59d14..8dddcbf3c26d53b8f306fa8024e4e02d2c75526b 100644
--- a/common/commands.c
+++ b/quad/src/commands/commands.c
@@ -72,6 +72,9 @@ command_cb cb_respoutput __attribute__((weak, alias("cb_default")));
 command_cb cb_getnodes __attribute__((weak, alias("cb_default")));
 command_cb cb_respnodes __attribute__((weak, alias("cb_default")));
 
+command_cb cb_addnode __attribute__((weak, alias("cb_default")));
+command_cb cb_respaddnode __attribute__((weak, alias("cb_default")));
+
 
 /*
  * Command structure.
@@ -140,14 +143,14 @@ struct MessageType MessageTypes[MAX_TYPE_ID] =
 		// Function pointer
 		&cb_log
 	},
-	// RESPONSE
+	// LOG_END
 	{
 		// Command text
-		"response",
+		"logend",
 		// Type of the command data
 		stringType,
 		// Function pointer
-		&cb_response
+		&cb_logend
 	},
 	// SETPARAM
 	{
@@ -210,7 +213,7 @@ struct MessageType MessageTypes[MAX_TYPE_ID] =
 		// Type of the command data
 		floatType,
 		// Function pointer
-		&cb_respoutput
+		&cb_getoutput
 	},
 	// RESPOUTPUT
 	{
@@ -239,14 +242,23 @@ struct MessageType MessageTypes[MAX_TYPE_ID] =
 		// Function pointer
 		&cb_respnodes
 	},
-	// LOG_END
+	// ADDNODE
 	{
 		// Command text
-		"logend",
+		"addnode",
 		// Type of the command data
-		stringType,
+		floatType,
 		// Function pointer
-		&cb_logend
+		&cb_addnode
+	},
+	// RESPADDNODE
+	{
+		// Command text
+		"respaddnode",
+		// Type of the command data
+		floatType,
+		// Function pointer
+		&cb_respaddnode
 	}
 };
 
diff --git a/common/commands.h b/quad/src/commands/commands.h
similarity index 57%
rename from common/commands.h
rename to quad/src/commands/commands.h
index 86fa2c897cf54f090af5e333ae7e755a64f30505..2d41d2e6294c74757518e51b8157df6351765667 100644
--- a/common/commands.h
+++ b/quad/src/commands/commands.h
@@ -5,8 +5,6 @@
 #include <stdlib.h>
 #include <string.h>
 
-#include "callbacks.h"
-
 #define MAX_CMD_TEXT_LENGTH 100
 
 enum Message{
@@ -41,20 +39,26 @@ enum MessageTypeID{
 	UPDATE_ID,            // 03
 	BEGINUPDATE_ID,       // 04
 	LOG_ID,               // 05
-	RESPONSE_ID,          // 06
-	SETPARAM_ID,          // 07 - Setting controller parameters. Example: PID constants
-	GETPARAM_ID,          // 08 - Getting controller parameters. Example: PID constants
-	RESPPARAM_ID,         // 09 - Responding with controller parameters. Example: PID constants
-	SETSOURCE_ID,         // 10 - Setting controller source Block & id
-	GETSOURCE_ID,         // 11 - Getting controller source Block & id
-	RESPSOURCE_ID,        // 12 - Responding with controller source Block & id
-	GETOUTPUT_ID,         // 13 - Getting controller block output
-	RESPOUTPUT_ID,        // 14 - Responding with controller block output
-	GETNODES_ID,          // 15 - Getting nodes from current comp_graph
-	RESPNODES_ID,         // 16 - Responding with nodes from current comp_graph 
-	LOG_END_ID,           // 17 - Responding with controller parameters. Example: PID constants
-	MAX_TYPE_ID           // 18 - Just used to keep track of the size. Must remain at the end
+	LOG_END_ID,           // 06 - Responding with controller parameters. Example: PID constants
+	SETPARAM_ID,          // 07 - Setting node parameters.
+	GETPARAM_ID,          // 08 - Getting node parameters.
+	RESPPARAM_ID,         // 09 - Responding with node parameters.
+	SETSOURCE_ID,         // 10 - Setting node input source block_id & output_id
+	GETSOURCE_ID,         // 11 - Getting node input source block_id & output_id
+	RESPSOURCE_ID,        // 12 - Responding with node input source block_id & output_id
+	GETOUTPUT_ID,         // 13 - Getting node output
+	RESPOUTPUT_ID,        // 14 - Responding with node output
+	GETNODES_ID,          // 15 - Getting node IDs from current comp_graph
+	RESPNODES_ID,         // 16 - Responding with node IDs from current comp_graph
+	ADDNODE_ID,			  // 17 - Add a node of specified type_id
+	RESPADDNODE_ID,		  // 18 - Responding with the block_id of the newly added node
+	MAX_TYPE_ID           // 19 - Just used to keep track of the size. Must remain at the end
 };
+
+struct modular_structs;
+struct metadata;
+typedef int (command_cb)(struct modular_structs *, struct metadata *, unsigned char *, unsigned short);
+
 /*
  * Message type struct used to keep track of the callback function
  * pointers located in commands.c
diff --git a/quad/src/computation_graph/Makefile b/quad/src/computation_graph/Makefile
index 5142ac39b839e02ca6ed2eb2c28fcec186b88ef7..ddd1f5c2e82619162c544e52c82cd774958782dd 100644
--- a/quad/src/computation_graph/Makefile
+++ b/quad/src/computation_graph/Makefile
@@ -1,6 +1,6 @@
 TOP=../..
 
 NAME = computation_graph
-REQLIBS = -ltest -lm
+REQLIBS = -ltest -lm -lgraph_blocks
 
 include $(TOP)/library.mk
diff --git a/quad/src/computation_graph/computation_graph.c b/quad/src/computation_graph/computation_graph.c
index 7434e14a112117b8dbe22a3ecd300b04c6c90413..7b21b2273e111b96160293108fa08aed2b7de5d5 100644
--- a/quad/src/computation_graph/computation_graph.c
+++ b/quad/src/computation_graph/computation_graph.c
@@ -81,6 +81,14 @@ int graph_set_source(struct computation_graph *graph,
     return 0;
 }
 
+struct node_src graph_get_source(struct computation_graph *graph, int node_id, int input_id) {
+    if (node_id >= graph->n_nodes || node_id < 0 ||
+        input_id >= graph->nodes[node_id].type->n_inputs || input_id < 0) {
+        return (struct node_src) {.controller_id = -1, .controller_output = -1};
+    }
+    return graph->nodes[node_id].input_srcs[input_id];
+}
+
 int graph_add_node(struct computation_graph *graph,
                    const char* name,
                    const struct graph_node_type *type,
@@ -104,7 +112,7 @@ int graph_add_node(struct computation_graph *graph,
     new_node->updated = 1;
     new_node->output_values = malloc(type->n_outputs * sizeof(double));
     new_node->param_values = calloc(type->n_params, sizeof(double));
-    new_node->input_srcs = malloc(type->n_inputs * sizeof(struct input_type));
+    new_node->input_srcs = malloc(type->n_inputs * sizeof(struct node_src));
     // Check that malloc succeeded in every case which memory was requested
     if ((type->n_outputs && !new_node->output_values) ||
         (type->n_params && !new_node->param_values) ||
diff --git a/quad/src/computation_graph/computation_graph.h b/quad/src/computation_graph/computation_graph.h
index e5e769fd28abcb90f66ef2df40434e132c496c62..e997aa7ab06a79d61aac152699d9a49379403ffc 100644
--- a/quad/src/computation_graph/computation_graph.h
+++ b/quad/src/computation_graph/computation_graph.h
@@ -25,14 +25,22 @@ struct computation_graph {
 
 // Declares a node type
 struct graph_node_type {
-    const char* const* input_names;
-    const char* const* output_names;
-    const char* const* param_names;
-    int n_inputs;
-    int n_outputs;
-    int n_params;
-    execute_node_t execute;
-    reset_node_t reset;
+    const char* const* input_names; // Array of strings corresponding to the inputs
+    const char* const* output_names; // Array of strings corresponding to the outputs
+    const char* const* param_names; // Array of strings corresponding to the parameters
+    int n_inputs; // Number of inputs
+    int n_outputs; // Number of outputs
+    int n_params; // Number of parameters
+    execute_node_t execute; // Function describing how to produce outputs
+    reset_node_t reset; // Reset this node. Called upon (re)connection
+    size_t state_size; // Size of the state struct for this type
+    int type_id; // A unique ID for this node type
+};
+
+// Holds a tuple for defining the source of a node. Includes the node ID and its output ID
+struct node_src { 
+    int controller_id;
+    int controller_output;
 };
 
 // Declares an instance of a node
@@ -44,10 +52,7 @@ struct graph_node {
     int n_children; // The number of connected children
     void *state; // Pointer to the state instance
     int processed_state; // State of the node with respecct to the graph traversal
-    struct input_type { // Array of tuples indicating the source for each input to this node
-        int controller_id;
-        int controller_output;
-    } *input_srcs;
+    struct node_src *input_srcs; // Array of tuples indicating the source for each input to this node
     int updated; // 1 if this node has had an input or parameter change
 };
 
@@ -68,6 +73,12 @@ struct computation_graph *create_graph();
  */
 int graph_set_source(struct computation_graph *graph, int dest_node_id, int dest_input, int src_node_id, int src_output);
 
+/*
+ * Returns the source node/output pair of a node's input. The mirror of graph_set_source.
+ * Returns a node ID of -1 if the requested node or input does not exist
+ */
+struct node_src graph_get_source(struct computation_graph *graph, int node_id, int input_id);
+
 /*
  * Creates a new node with the given data, and adds it to the graph.
  * Returns a negative integer upon failure.
diff --git a/quad/src/computation_graph/test/test_computation_graph.c b/quad/src/computation_graph/test/test_computation_graph.c
index 1948421eb7c8792756c51e4cccd6f8c559f0ac95..929fe9ca44404efbaa9e46287cc78a6a2fe057f9 100644
--- a/quad/src/computation_graph/test/test_computation_graph.c
+++ b/quad/src/computation_graph/test/test_computation_graph.c
@@ -2,11 +2,7 @@
 
 
 #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.h"
 
 #define GRAPH_TEST_EPS 0.00001
 
@@ -19,10 +15,10 @@ static int nequal(double val1, double val2) {
 
 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");
+    int block = graph_add_defined_block(graph, BLOCK_ADD, "Add");
+    int cblock3 = graph_add_defined_block(graph, BLOCK_CONSTANT, "3");
     graph_set_param_val(graph, cblock3, CONST_SET, 3);
-    int cblock4 = graph_add_node_const(graph, "4");
+    int cblock4 = graph_add_defined_block(graph, BLOCK_CONSTANT, "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);
@@ -35,8 +31,8 @@ int graph_test_one_add() {
 
 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");
+    int gain1 = graph_add_defined_block(graph, BLOCK_GAIN, "gain1");
+    int gain2 = graph_add_defined_block(graph, BLOCK_GAIN, "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};
@@ -47,8 +43,8 @@ int graph_test_circular_runs() {
 
 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");
+    int acum1 = graph_add_defined_block(graph, BLOCK_ACCUMULATE, "accumulator1");
+    int acum2 = graph_add_defined_block(graph, BLOCK_ACCUMULATE, "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
@@ -57,8 +53,8 @@ int graph_test_circular_resets() {
 // 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");
+    int cblock = graph_add_defined_block(graph, BLOCK_CONSTANT, "const");
+    int acum_b = graph_add_defined_block(graph, BLOCK_ACCUMULATE, "accumulator");
     graph_set_source(graph, acum_b, ACCUM_IN, cblock, CONST_VAL);
 
     int to_compute_for[1] = {acum_b};
@@ -76,7 +72,7 @@ int graph_test_accumulator() {
     }
 
     // Test reset on source set
-    int gain_b = graph_add_node_gain(graph, "Gain");
+    int gain_b = graph_add_defined_block(graph, BLOCK_GAIN, "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;
@@ -93,9 +89,9 @@ int graph_test_accumulator() {
 // 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");
+    int acum_b = graph_add_defined_block(graph, BLOCK_ACCUMULATE, "accumulator");
+    int add_block = graph_add_defined_block(graph, BLOCK_ADD, "Add");
+    int cblock = graph_add_defined_block(graph, BLOCK_CONSTANT, "const");
     graph_set_param_val(graph, cblock, CONST_SET, 2);
 
 
@@ -112,10 +108,10 @@ int graph_test_single_run() {
 // 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");
+    int cblock = graph_add_defined_block(graph, BLOCK_CONSTANT, "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");
+    int acum_b = graph_add_defined_block(graph, BLOCK_ACCUMULATE, "accumulator");
+    int gain1 = graph_add_defined_block(graph, BLOCK_GAIN, "gain1");
     graph_set_param_val(graph, gain1, GAIN_GAIN, 1);
 
     graph_set_source(graph, gain1, GAIN_INPUT, acum_b, ACCUMULATED);
@@ -124,7 +120,7 @@ int graph_test_reset_rules() {
     graph_compute_nodes(graph, to_compute_for, 1);
     // state of acum_b is now 5
 
-    int gain2 = graph_add_node_gain(graph, "gain2");
+    int gain2 = graph_add_defined_block(graph, BLOCK_GAIN, "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);
@@ -138,7 +134,7 @@ int graph_test_reset_rules() {
 
 int graph_test_self_loop() {
     struct computation_graph *graph = create_graph();
-    int gain1 = graph_add_node_gain(graph, "gain1");
+    int gain1 = graph_add_defined_block(graph, BLOCK_GAIN, "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);
@@ -147,8 +143,8 @@ int graph_test_self_loop() {
 
 int graph_test_update_rules() {
     struct computation_graph *graph = create_graph();
-    int cblock = graph_add_node_const(graph, "const");
-    int acum_b = graph_add_node_accum(graph, "accumulator");
+    int cblock = graph_add_defined_block(graph, BLOCK_CONSTANT, "const");
+    int acum_b = graph_add_defined_block(graph, BLOCK_ACCUMULATE, "accumulator");
     graph_set_source(graph, acum_b, ACCUM_IN, cblock, CONST_VAL);
 
     graph_set_param_val(graph, cblock, CONST_SET, 3);
@@ -167,11 +163,11 @@ C2 --->| accum_b2 --->|
 */
 int graph_test_update_propagation() {
     struct computation_graph *graph = create_graph();
-    int cblock1 = graph_add_node_const(graph, "const1");
-    int cblock2 = graph_add_node_const(graph, "const2");
-    int accum_b1 = graph_add_node_accum(graph, "accumulator1");
-    int accum_b2 = graph_add_node_accum(graph, "accumulator2");
-    int add_b = graph_add_node_add(graph, "add");
+    int cblock1 = graph_add_defined_block(graph, BLOCK_CONSTANT, "const1");
+    int cblock2 = graph_add_defined_block(graph, BLOCK_CONSTANT, "const2");
+    int accum_b1 = graph_add_defined_block(graph, BLOCK_ACCUMULATE, "accumulator1");
+    int accum_b2 = graph_add_defined_block(graph, BLOCK_ACCUMULATE, "accumulator2");
+    int add_b = graph_add_defined_block(graph, BLOCK_ADD, "add");
     graph_set_source(graph, accum_b1, ACCUM_IN, cblock1, CONST_VAL);
     graph_set_source(graph, accum_b2, ACCUM_IN, cblock2, CONST_VAL);
     graph_set_source(graph, add_b, ADD_SUMMAND1, accum_b1, ACCUMULATED);
@@ -199,10 +195,10 @@ the node would never get set.
 int graph_test_update_disconnected() {
     printf("\n\n---------\n");
     struct computation_graph *graph = create_graph();
-    int d_block = graph_add_node_const(graph, "const1");
-    int gain_block = graph_add_node_gain(graph, "gain");
-    int gain2_block = graph_add_node_gain(graph, "gain2");
-    int const_b = graph_add_node_const(graph, "const2");
+    int d_block = graph_add_defined_block(graph, BLOCK_CONSTANT, "const1");
+    int gain_block = graph_add_defined_block(graph, BLOCK_GAIN, "gain");
+    int gain2_block = graph_add_defined_block(graph, BLOCK_GAIN, "gain2");
+    int const_b = graph_add_defined_block(graph, BLOCK_CONSTANT, "const2");
     graph_set_source(graph, gain_block, GAIN_INPUT, const_b, CONST_VAL);
     graph_set_source(graph, gain2_block, GAIN_INPUT, d_block, CONST_VAL); // We need this so d_block doesn't get updated
     graph_set_param_val(graph, gain_block, GAIN_GAIN, 2);
@@ -217,8 +213,35 @@ int graph_test_update_disconnected() {
     return nequal(set_val, 2*1.2345);
 }
 
+int graph_test_get_source() {
+    struct computation_graph *graph = create_graph();
+    int add_block = graph_add_defined_block(graph, BLOCK_ADD, "Add");
+    int cblock3 = graph_add_defined_block(graph, BLOCK_CONSTANT, "3");
+    graph_set_source(graph, add_block, ADD_SUMMAND1, cblock3, CONST_VAL);
+
+    struct node_src source = graph_get_source(graph, add_block, ADD_SUMMAND1);
+    if (source.controller_id != cblock3 || source.controller_output != CONST_VAL) {
+        return 1;
+    }
+    else {
+        return 0;
+    }
+}
+
+int graph_test_get_source_null() {
+    struct computation_graph *graph = create_graph();
+    int add_block = graph_add_defined_block(graph, BLOCK_ADD, "Add");
+
+    struct node_src source = graph_get_source(graph, 123, ADD_SUMMAND1);
+    if (source.controller_id != -1) {
+        return 1;
+    }
+    else {
+        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");
@@ -229,5 +252,7 @@ int main() {
     test(graph_test_update_rules, "Tests that nodes only update when their inputs change");
     test(graph_test_update_propagation, "Tests that updates propagate only to their children");
     test(graph_test_update_disconnected, "Tests that nodes get executed when updated, even if disconnected");
+    test(graph_test_get_source, "Tests that the get_source call works normally");
+    test(graph_test_get_source_null, "Tests that the get_source call returns ID -1 when invalid ID is passed");
     return test_summary();
 }
diff --git a/quad/src/graph_blocks/.gitignore b/quad/src/graph_blocks/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..90edb74c26197159cf7438929580622073b7cd17
--- /dev/null
+++ b/quad/src/graph_blocks/.gitignore
@@ -0,0 +1 @@
+obj/
diff --git a/quad/src/graph_blocks/Makefile b/quad/src/graph_blocks/Makefile
new file mode 100644
index 0000000000000000000000000000000000000000..2769a61ebc43c61087f2cb47df50d11e94729e95
--- /dev/null
+++ b/quad/src/graph_blocks/Makefile
@@ -0,0 +1,6 @@
+TOP=../..
+
+NAME = graph_blocks
+REQLIBS = -lcomputation_graph
+
+include $(TOP)/library.mk
diff --git a/quad/src/graph_blocks/graph_blocks.c b/quad/src/graph_blocks/graph_blocks.c
new file mode 100644
index 0000000000000000000000000000000000000000..325218bf8d966c0eaca77675fb091ad34e4a28fd
--- /dev/null
+++ b/quad/src/graph_blocks/graph_blocks.c
@@ -0,0 +1,33 @@
+#include "graph_blocks.h"
+#include <stdlib.h>
+
+// See graph_blocks.h
+const struct graph_node_type* blockDefs[MAX_BLOCK_TYPES] = 
+{
+    &node_const_type,
+    &node_add_type,
+    &node_mult_type,
+    &node_gain_type,
+    &node_accum_type,
+    &node_bounds_type,
+    &node_mixer_type,
+    &node_pid_type
+};
+
+
+int graph_add_defined_block(struct computation_graph* graph, int type_id, const char* name) {
+    // Verify block type is valid
+    if (type_id >= MAX_BLOCK_TYPES) {
+        return -1;
+    }
+    const struct graph_node_type *block_type = blockDefs[type_id];
+    void* state = NULL;
+    // Allocate the state struct for this node, if necessary
+    if (block_type->state_size) {
+        state = malloc(block_type->state_size);
+        if (!state) {return -1;} // Check for malloc failure
+    }
+
+    // Use the computation graph implementation's add node function
+    return graph_add_node(graph, name, block_type, state);
+}
\ No newline at end of file
diff --git a/quad/src/graph_blocks/graph_blocks.h b/quad/src/graph_blocks/graph_blocks.h
new file mode 100644
index 0000000000000000000000000000000000000000..812e2a844a15ed0afba19d7fd3f2b78b8a5ea997
--- /dev/null
+++ b/quad/src/graph_blocks/graph_blocks.h
@@ -0,0 +1,54 @@
+#ifndef __GRAPH_BLOCKS_H__
+#define __GRAPH_BLOCKS_H__
+
+#include "computation_graph.h"
+#include "node_constant.h"
+#include "node_add.h"
+#include "node_mult.h"
+#include "node_gain.h"
+#include "node_accumulator.h"
+#include "node_bounds.h"
+#include "node_mixer.h"
+#include "node_pid.h"
+
+/*
+ * ---------- How-To ------------
+ * To add a new block type, put the implementation (.c and .h) files
+ * in the same directory as this file, and include the header file above
+ * Add a new entry to this enum right before MAX_BLOCK_TYPES
+ * 
+ * In graph_blocks.c, add a new entry at the end of the array with
+ * your graph_node_type struct.
+ */
+
+
+/*
+ * Enumerates all the types of different block types.
+ * Must match blockDefs
+ */
+enum BlockTypes {
+    BLOCK_CONSTANT,      // 00
+    BLOCK_ADD,           // 01
+    BLOCK_MULT,          // 02
+    BLOCK_GAIN,          // 03
+    BLOCK_ACCUMULATE,    // 04
+    BLOCK_BOUNDS,        // 05
+    BLOCK_MIXER,         // 06
+    BLOCK_PID,           // 07
+    //                        <-- Insert new block type here
+    MAX_BLOCK_TYPES
+};
+
+/*
+ * Array corresponding to the different block type structs
+ * Mustm match the nums in BlockTypes
+ */
+extern const struct graph_node_type* blockDefs[MAX_BLOCK_TYPES];
+
+
+/*
+ * Creates a new node and adds it to the graph with the given type ID and name
+ * Returns the id of the new node upon success, -1 upon failure
+ */
+int graph_add_defined_block(struct computation_graph* graph, int type_id, const char* name);
+#endif // __GRAPH_BLOCKS_H__
diff --git a/quad/src/computation_graph/node_accumulator.c b/quad/src/graph_blocks/node_accumulator.c
similarity index 91%
rename from quad/src/computation_graph/node_accumulator.c
rename to quad/src/graph_blocks/node_accumulator.c
index 17a36ee92dffc796e9ea592f28ecd1f4737085a1..10b96873750e862b9072d0f2738842aa3be69ec7 100644
--- a/quad/src/computation_graph/node_accumulator.c
+++ b/quad/src/graph_blocks/node_accumulator.c
@@ -27,7 +27,9 @@ const struct graph_node_type node_accum_type = {
         .n_outputs = 1,
         .n_params = 0,
         .execute = accum_nodes,
-        .reset = reset
+        .reset = reset,
+        .state_size = sizeof(struct accum_state),
+        .type_id = BLOCK_ACCUMULATE
 };
 
 int graph_add_node_accum(struct computation_graph *graph, const char* name) {
diff --git a/quad/src/computation_graph/node_accumulator.h b/quad/src/graph_blocks/node_accumulator.h
similarity index 93%
rename from quad/src/computation_graph/node_accumulator.h
rename to quad/src/graph_blocks/node_accumulator.h
index a392a9db295e5bc7f44d84292e4aa561ee3fba9d..43301d6676aa1289a6523da45546ffc88f7cbfe0 100644
--- a/quad/src/computation_graph/node_accumulator.h
+++ b/quad/src/graph_blocks/node_accumulator.h
@@ -1,6 +1,7 @@
 #ifndef __NODE_ACCUMULATOR_H__
 #define __NODE_ACCUMULATOR_H__
 #include "computation_graph.h"
+#include "graph_blocks.h"
 
 int graph_add_node_accum(struct computation_graph *graph, const char* name);
 
diff --git a/quad/src/computation_graph/node_add.c b/quad/src/graph_blocks/node_add.c
similarity index 91%
rename from quad/src/computation_graph/node_add.c
rename to quad/src/graph_blocks/node_add.c
index 048a49b3b6a9c054f74900e356e6377f17a6a4b7..25a780a32681df3d56aec758f9f0527115ee00b7 100644
--- a/quad/src/computation_graph/node_add.c
+++ b/quad/src/graph_blocks/node_add.c
@@ -17,7 +17,9 @@ const struct graph_node_type node_add_type = {
         .n_outputs = 1,
         .n_params = 0,
         .execute = add_nodes,
-        .reset = reset
+        .reset = reset,
+        .state_size = 0,
+        .type_id = BLOCK_ADD
 };
 
 int graph_add_node_add(struct computation_graph *graph, const char* name) {
diff --git a/quad/src/computation_graph/node_add.h b/quad/src/graph_blocks/node_add.h
similarity index 92%
rename from quad/src/computation_graph/node_add.h
rename to quad/src/graph_blocks/node_add.h
index 390e3d229c7a943fdd09c26e18be711672cf000e..34b8123638dc1d954ceb0dcd6ceadb959ae6c5c5 100644
--- a/quad/src/computation_graph/node_add.h
+++ b/quad/src/graph_blocks/node_add.h
@@ -1,6 +1,7 @@
 #ifndef __NODE_ADD_H__
 #define __NODE_ADD_H__
 #include "computation_graph.h"
+#include "graph_blocks.h"
 
 int graph_add_node_add(struct computation_graph *graph, const char* name);
 
diff --git a/quad/src/quad_app/node_bounds.c b/quad/src/graph_blocks/node_bounds.c
similarity index 92%
rename from quad/src/quad_app/node_bounds.c
rename to quad/src/graph_blocks/node_bounds.c
index aae4a67957a3d76c89d44480962e7b426b94f235..d1fe264311e1c5223df17f61e7e0330e9466c286 100644
--- a/quad/src/quad_app/node_bounds.c
+++ b/quad/src/graph_blocks/node_bounds.c
@@ -25,7 +25,9 @@ const struct graph_node_type node_bounds_type = {
         .n_outputs = 1,
         .n_params = 2,
         .execute = bounds_computation,
-        .reset = reset_bounds
+        .reset = reset_bounds,
+        .state_size = 0,
+        .type_id = BLOCK_BOUNDS
 };
 
 int graph_add_node_bounds(struct computation_graph *graph, const char* name) {
diff --git a/quad/src/quad_app/node_bounds.h b/quad/src/graph_blocks/node_bounds.h
similarity index 94%
rename from quad/src/quad_app/node_bounds.h
rename to quad/src/graph_blocks/node_bounds.h
index 4c3b93256106bce8560daf28c67f19844fb2ea8f..7eea073fa6000457c30588f251565fcdd3db1a5c 100644
--- a/quad/src/quad_app/node_bounds.h
+++ b/quad/src/graph_blocks/node_bounds.h
@@ -1,6 +1,7 @@
 #ifndef __NODE_BOUNDS_H__
 #define __NODE_BOUNDS_H__
 #include "computation_graph.h"
+#include "graph_blocks.h"
 
 int graph_add_node_bounds(struct computation_graph *graph, const char* name);
 
diff --git a/quad/src/computation_graph/node_constant.c b/quad/src/graph_blocks/node_constant.c
similarity index 90%
rename from quad/src/computation_graph/node_constant.c
rename to quad/src/graph_blocks/node_constant.c
index b2a46e757eef397b4c9805d4b76a159fccc6c9a0..8bd406ad924f3025631c4d251c5d751eddcce90e 100644
--- a/quad/src/computation_graph/node_constant.c
+++ b/quad/src/graph_blocks/node_constant.c
@@ -17,7 +17,9 @@ const struct graph_node_type node_const_type = {
         .n_outputs = 1,
         .n_params = 1,
         .execute = output_const,
-        .reset = reset
+        .reset = reset,
+        .state_size = 0,
+        .type_id = BLOCK_CONSTANT
 };
 
 int graph_add_node_const(struct computation_graph *graph, const char* name) {
diff --git a/quad/src/computation_graph/node_constant.h b/quad/src/graph_blocks/node_constant.h
similarity index 93%
rename from quad/src/computation_graph/node_constant.h
rename to quad/src/graph_blocks/node_constant.h
index c67ac6e0511e971e1f9ca57c853bd9de6fca6238..417e92da19bbe1170cbec09598e4129d3c4400a6 100644
--- a/quad/src/computation_graph/node_constant.h
+++ b/quad/src/graph_blocks/node_constant.h
@@ -1,6 +1,7 @@
 #ifndef __NODE_CONSTANT_H__
 #define __NODE_CONSTANT_H__
 #include "computation_graph.h"
+#include "graph_blocks.h"
 
 int graph_add_node_const(struct computation_graph *graph, const char* name);
 
diff --git a/quad/src/computation_graph/node_gain.c b/quad/src/graph_blocks/node_gain.c
similarity index 91%
rename from quad/src/computation_graph/node_gain.c
rename to quad/src/graph_blocks/node_gain.c
index 35bb3702cf2aa36d2f08f85ed580d12fadbe8fa8..3c0ac77ae8a54b0e99b6b767ec85083f3e047e91 100644
--- a/quad/src/computation_graph/node_gain.c
+++ b/quad/src/graph_blocks/node_gain.c
@@ -17,7 +17,9 @@ const struct graph_node_type node_gain_type = {
         .n_outputs = 1,
         .n_params = 1,
         .execute = scale_nodes,
-        .reset = reset
+        .reset = reset,
+        .state_size = 0,
+        .type_id = BLOCK_GAIN
 };
 
 int graph_add_node_gain(struct computation_graph *graph, const char* name) {
diff --git a/quad/src/computation_graph/node_gain.h b/quad/src/graph_blocks/node_gain.h
similarity index 93%
rename from quad/src/computation_graph/node_gain.h
rename to quad/src/graph_blocks/node_gain.h
index 4089244980ad7ee2fc50bc375f6cbb1500196b5e..4a1a3322f65789ffccd6282125181f8deade4200 100644
--- a/quad/src/computation_graph/node_gain.h
+++ b/quad/src/graph_blocks/node_gain.h
@@ -1,6 +1,7 @@
 #ifndef __NODE_GAIN_H__
 #define __NODE_GAIN_H__
 #include "computation_graph.h"
+#include "graph_blocks.h"
 
 int graph_add_node_gain(struct computation_graph *graph, const char* name);
 
diff --git a/quad/src/quad_app/node_mixer.c b/quad/src/graph_blocks/node_mixer.c
similarity index 95%
rename from quad/src/quad_app/node_mixer.c
rename to quad/src/graph_blocks/node_mixer.c
index 316c1ce8218b835ee002c8c4352c97af111b318d..fe7d57f276106e7847e5c0b0c0ba0fc92f2ed862 100644
--- a/quad/src/quad_app/node_mixer.c
+++ b/quad/src/graph_blocks/node_mixer.c
@@ -34,7 +34,9 @@ const struct graph_node_type node_mixer_type = {
         .n_outputs = 4,
         .n_params = 0,
         .execute = mixer_computation,
-        .reset = reset_mixer
+        .reset = reset_mixer,
+		.state_size = 0,
+		.type_id = BLOCK_MIXER
 };
 
 int graph_add_node_mixer(struct computation_graph *graph, const char* name) {
diff --git a/quad/src/quad_app/node_mixer.h b/quad/src/graph_blocks/node_mixer.h
similarity index 94%
rename from quad/src/quad_app/node_mixer.h
rename to quad/src/graph_blocks/node_mixer.h
index ba1ec1c4901db366610506f18e41f9883a41980b..584538f0242677428632b681b2f0fe064e789abc 100644
--- a/quad/src/quad_app/node_mixer.h
+++ b/quad/src/graph_blocks/node_mixer.h
@@ -1,6 +1,7 @@
 #ifndef __NODE_MIXER_H__
 #define __NODE_MIXER_H__
 #include "computation_graph.h"
+#include "graph_blocks.h"
 
 int graph_add_node_mixer(struct computation_graph *graph, const char* name);
 
diff --git a/quad/src/computation_graph/node_mult.c b/quad/src/graph_blocks/node_mult.c
similarity index 91%
rename from quad/src/computation_graph/node_mult.c
rename to quad/src/graph_blocks/node_mult.c
index 2696719fdc83a89ae346d4dc788df693d97b2ddb..427b9e74d071b45bbd5c7ef2437647f13aeb5b5c 100644
--- a/quad/src/computation_graph/node_mult.c
+++ b/quad/src/graph_blocks/node_mult.c
@@ -17,7 +17,9 @@ const struct graph_node_type node_mult_type = {
         .n_outputs = 1,
         .n_params = 0,
         .execute = mult_nodes,
-        .reset = reset
+        .reset = reset,
+        .state_size = 0,
+        .type_id = BLOCK_MULT
 };
 
 int graph_add_node_mult(struct computation_graph *graph, const char* name) {
diff --git a/quad/src/computation_graph/node_mult.h b/quad/src/graph_blocks/node_mult.h
similarity index 93%
rename from quad/src/computation_graph/node_mult.h
rename to quad/src/graph_blocks/node_mult.h
index 9ea2bbb9fabdbc522a02f292f5294b33ad7af44c..f86f7ee0ab88c64dc1f3241615ac06d66fc3d925 100644
--- a/quad/src/computation_graph/node_mult.h
+++ b/quad/src/graph_blocks/node_mult.h
@@ -1,6 +1,7 @@
 #ifndef __NODE_MULT_H__
 #define __NODE_MULT_H__
 #include "computation_graph.h"
+#include "graph_blocks.h"
 
 int graph_add_node_mult(struct computation_graph *graph, const char* name);
 
diff --git a/quad/src/quad_app/node_pid.c b/quad/src/graph_blocks/node_pid.c
similarity index 97%
rename from quad/src/quad_app/node_pid.c
rename to quad/src/graph_blocks/node_pid.c
index 7222094577d6c4ba1f41c93f65e5bdbe00c4dc75..df6b611100aec808a8728a3fbe435ecbe5078557 100644
--- a/quad/src/quad_app/node_pid.c
+++ b/quad/src/graph_blocks/node_pid.c
@@ -90,7 +90,9 @@ const struct graph_node_type node_pid_type = {
         .n_outputs = 1,
         .n_params = 4,
         .execute = pid_computation,
-        .reset = reset_pid
+        .reset = reset_pid,
+        .state_size = sizeof(struct pid_node_state),
+        .type_id = BLOCK_PID
 };
 
 int graph_add_node_pid(struct computation_graph *graph, const char* name) {
diff --git a/quad/src/quad_app/node_pid.h b/quad/src/graph_blocks/node_pid.h
similarity index 96%
rename from quad/src/quad_app/node_pid.h
rename to quad/src/graph_blocks/node_pid.h
index 7b384bb1a233785877fc2cc79d56a8e2dd88aa10..694f7a143d5141d7bbcaf951302fee187f56ab15 100644
--- a/quad/src/quad_app/node_pid.h
+++ b/quad/src/graph_blocks/node_pid.h
@@ -1,6 +1,7 @@
 #ifndef __NODE_PID_H__
 #define __NODE_PID_H__
 #include "computation_graph.h"
+#include "graph_blocks.h"
 
 int graph_add_node_pid(struct computation_graph *graph, const char* name);
 
diff --git a/quad/src/computation_graph/node_pow.c b/quad/src/graph_blocks/node_pow.c
similarity index 94%
rename from quad/src/computation_graph/node_pow.c
rename to quad/src/graph_blocks/node_pow.c
index dbbc1d707d7833254e5a96a4dc8ba2b9c954ae4d..e4fe0f25c2dd323085110869e3f1191217914d40 100644
--- a/quad/src/computation_graph/node_pow.c
+++ b/quad/src/graph_blocks/node_pow.c
@@ -18,7 +18,8 @@ const struct graph_node_type node_pow_type = {
         .n_outputs = 1,
         .n_params = 1,
         .execute = pow_nodes,
-        .reset = reset
+        .reset = reset,
+        .state_size = 0,
 };
 
 int graph_add_node_pow(struct computation_graph *graph, const char* name) {
diff --git a/quad/src/computation_graph/node_pow.h b/quad/src/graph_blocks/node_pow.h
similarity index 100%
rename from quad/src/computation_graph/node_pow.h
rename to quad/src/graph_blocks/node_pow.h
diff --git a/quad/src/quad_app/Makefile b/quad/src/quad_app/Makefile
index aea2974582bf7a794b8b32ecdd46223260d26470..02cc8d007386acd8b03e61623248cd1e75059829 100644
--- a/quad/src/quad_app/Makefile
+++ b/quad/src/quad_app/Makefile
@@ -1,6 +1,6 @@
 TOP=../..
 
 NAME = quad_app
-REQLIBS = -ltest -lcomputation_graph -lm -lqueue
+REQLIBS = -ltest -lcomputation_graph -lm -lqueue -lgraph_blocks -lcommands
 
 include $(TOP)/library.mk
diff --git a/quad/src/quad_app/callbacks.c b/quad/src/quad_app/callbacks.c
index 183ecf85458ee8233f30ab0c6afdc688cfc7a983..6ddd546a344df78d637707399cf600472040df53 100644
--- a/quad/src/quad_app/callbacks.c
+++ b/quad/src/quad_app/callbacks.c
@@ -99,41 +99,55 @@ int cb_beginupdate(modular_structs_t *structs, metadata_t *meta, u8 *data, u16 l
 	return 0;
 }
 
+/*
+ * -----------------------------------------------
+ * Callbacks for control network modification/info
+ * -----------------------------------------------
+*/
 
-/* Callbacks for configuration */
+// Struct for holding a node ID and either a parameter, output, or input index
+struct node_ids {
+	int16_t id;
+	int16_t sub_id;
+};
+
+/*
+ * Given a data array, returns a node_ids struct retrieved from the array.
+ * Assumes the given array is at least 4 bytes to hold the data.
+*/
+struct node_ids get_node_ids(u8 *data) {
+	return (struct node_ids) {
+		.id = build_short(data),
+		.sub_id = build_short(data + 2)
+	};
+}
 
 /**
-  * Handles a command to set a controller parameter on the quad.
+  * Handles a command to set a node parameter on the quad.
   *
   * NOTE:
   * Expects the uart buff to have data in the following format:
   * |--------------------------------------------------------|
-  * |  data index ||      0      |      1      |    2 - 5    |
+  * |  data index ||    0 - 1    |    2 - 3    |    4 - 7    |
   * |--------------------------------------------------------|
-  * |   parameter ||  control ID | ctrl parmID |  param val  |
+  * |   parameter ||   node ID   | node parmID |  param val  |
   * |--------------------------------------------------------|
-  * |       bytes ||      1      |      1      |      4      |
+  * |       bytes ||      2      |      2      |      4      |
   * |--------------------------------------------------------|
   * 
   * Does not send anything in response.
   */
 int cb_setparam(modular_structs_t *structs, metadata_t *meta, u8 *data, u16 length)
 {
-	// Get some of the meta data
-	u16 data_len = length;
 	// Check if the data length is correct
-	if (data_len != 6)
-	{
-		return -1;
-	}
+	if (length != 8) {return -1;}
 	struct computation_graph* graph = structs->parameter_struct.graph;
 
 	// Get the node ID, parameter ID, parameter value
-	u8 node_id = data[0];
-	u8 param_id = data[1];
-	float param_val = build_float(data + 2);
+	struct node_ids ids = get_node_ids(data);
+	float param_val = build_float(data + 4);
 	// Set the value for that parameter on that node
-	graph_set_param_val(graph, node_id, param_id, param_val);
+	graph_set_param_val(graph, ids.id, ids.sub_id, param_val);
 
 	return 0;
 }
@@ -143,54 +157,234 @@ int cb_setparam(modular_structs_t *structs, metadata_t *meta, u8 *data, u16 leng
   *
   * NOTE:
   * Expects the uart buff to have data in the following format:
-  * |------------------------------------------|
-  * |  data index ||      0      |      1      |
-  * |------------------------------------------|
-  * |   parameter ||  control ID | ctrl parmID |
-  * |------------------------------------------|
-  * |       bytes ||      1      |      1      |
-  * |------------------------------------------|
+  * |-------------------------------------------|
+  * |  data index ||    0 - 1     |    2 - 3    |
+  * |-------------------------------------------|
+  * |   parameter ||    node ID   | node parmID |
+  * |-------------------------------------------|
+  * |       bytes ||       2      |      2      |
+  * |-------------------------------------------|
   *
-  * Sends a response of type RESPONSECONTROL_ID.
+  * Sends a response of type RESPPARAM_ID.
   * The response will have a message ID equal to the one originally received.
   * The data of the response will be in the following format:
   * |--------------------------------------------------------|
-  * |  data index ||      0      |      1      |    2 - 5    |
+  * |  data index ||    0 - 1    |    2 - 3    |    4 - 7    |
   * |--------------------------------------------------------|
-  * |   parameter ||  control ID | ctrl parmID |  param val  |
+  * |   parameter ||   node ID   | node parmID |  param val  |
   * |--------------------------------------------------------|
-  * |       bytes ||      1      |      1      |      4      |
+  * |       bytes ||      2      |      2      |      4      |
   * |--------------------------------------------------------|
   */
 int cb_getparam(modular_structs_t* structs, metadata_t *meta,  u8 *data, u16 length)
 {
-	// Get some of the meta data
-	u16 data_len = length;
-	u16 msg_id = meta->msg_id;
 	// Check if the data length is correct
-	if (data_len != 2)
-	{
-		return -1;
-	}
+	if (length != 4) {return -1;}
+	u16 msg_id = meta->msg_id;
 
 	// Get the controller ID, parameter ID
-	u8 node_id = data[0];
-	u8 param_id = data[1];
+	struct node_ids ids = get_node_ids(data);
 	struct computation_graph* graph = structs->parameter_struct.graph;
-	float param_val = graph_get_param_val(graph, node_id, param_id);
+	float param_val = graph_get_param_val(graph, ids.id, ids.sub_id);
 
 	// Format the response data
-	char resp_data[6];
+	char resp_data[8];
 	// Controller ID
-	resp_data[0] = node_id;
+	pack_short(ids.id, resp_data);
 	// Parameter ID
-	resp_data[1] = param_id;
+	pack_short(ids.sub_id, resp_data + 2);
 	// Parameter value (4 byte float)
-	// TODO set a strict byte ordering for communication between the ground station and the quad
-	memcpy(&resp_data[2], &param_val, sizeof(param_val));
+	pack_float(param_val, resp_data + 4);
 
 	// Send the response
 	send_data(&structs->hardware_struct.uart, RESPPARAM_ID, msg_id, resp_data, sizeof(resp_data));
 
 	return 0;
 }
+
+/**
+  * Handles a command to set a node's input source
+  *
+  * NOTE:
+  * Expects the uart buff to have data in the following format:
+  * |---------------------------------------------------------------------------|
+  * |  data index ||     0 - 1    |     2 - 3     |    4 - 5    |     6 - 7     |
+  * |---------------------------------------------------------------------------|
+  * |   parameter || dest node ID | dest input ID | src node ID | src output ID |
+  * |---------------------------------------------------------------------------|
+  * |       bytes ||       2      |       2       |      2      |       2       |
+  * |---------------------------------------------------------------------------|
+  * 
+  * Does not send anything in response.
+  */
+int cb_setsource(modular_structs_t* structs, metadata_t *meta,  u8 *data, u16 length) {
+	if (length != 8) {return -1;}
+	int16_t dest_node = build_short(data);
+	int16_t dest_input = build_short(data + 2);
+	int16_t src_node = build_short(data + 4);
+	int16_t src_input = build_short(data + 6);
+
+	struct computation_graph* graph = structs->parameter_struct.graph;
+	graph_set_source(graph, dest_node, dest_input, src_node, src_input);
+
+	return 0;
+}
+
+/**
+  * Handles a command to get the source of a node's input
+  *
+  * NOTE:
+  * Expects the uart buff to have data in the following format:
+  * |---------------------------------------------|
+  * |  data index ||    0 - 1     |     2 - 3     |
+  * |---------------------------------------------|
+  * |   parameter ||    node ID   | node input ID |
+  * |---------------------------------------------|
+  * |       bytes ||       2      |       2       |
+  * |---------------------------------------------|
+  *
+  * Sends a response of type RESPSOURCE_ID.
+  * The response will have a message ID equal to the one originally received.
+  * The data of the response will be in the following format:
+  * |---------------------------------------------------------------------------|
+  * |  data index ||     0 - 1    |     2 - 3     |    4 - 5    |     6 - 7     |
+  * |---------------------------------------------------------------------------|
+  * |   parameter || dest node ID | dest input ID | src node ID | src output ID |
+  * |---------------------------------------------------------------------------|
+  * |       bytes ||       2      |       2       |      2      |       2       |
+  * |---------------------------------------------------------------------------|
+  */
+int cb_getsource(modular_structs_t* structs, metadata_t *meta,  u8 *data, u16 length) {
+	if (length != 4) {return -1;}
+	u16 msg_id = meta->msg_id;
+	// Get requested IDs
+	struct node_ids ids = get_node_ids(data);
+
+	u8 resp_data[8];
+	pack_short(ids.id, resp_data);
+	pack_short(ids.sub_id, resp_data + 2);
+
+	struct computation_graph* graph = structs->parameter_struct.graph;
+	struct node_src source = graph_get_source(graph, ids.id, ids.sub_id);
+	pack_short(source.controller_id, resp_data + 4);
+	pack_short(source.controller_output, resp_data + 6);
+
+	send_data(&structs->hardware_struct.uart, RESPSOURCE_ID, msg_id, resp_data, sizeof(resp_data));
+}
+
+/**
+  * Handles a command to get a node output value from the quad.
+  * Packet structure is the same as getparam
+  */
+int cb_getoutput(modular_structs_t* structs, metadata_t *meta,  u8 *data, u16 length)
+{
+	// Check if the data length is correct
+	if (length != 4) {return -1;}
+	u16 msg_id = meta->msg_id;
+
+	// Get the controller ID, parameter ID
+	struct node_ids ids = get_node_ids(data);
+	struct computation_graph* graph = structs->parameter_struct.graph;
+	float output_val = graph_get_output(graph, ids.id, ids.sub_id);
+
+	// Format the response data
+	char resp_data[8];
+	// Controller ID
+	pack_short(ids.id, resp_data);
+	// Output ID
+	pack_short(ids.sub_id, resp_data + 2);
+	// Output value (4 byte float)
+	pack_float(output_val, resp_data + 4);
+
+	// Send the response
+	send_data(&structs->hardware_struct.uart, RESPOUTPUT_ID, msg_id, resp_data, sizeof(resp_data));
+
+	return 0;
+}
+
+/*
+ * Handles a request for the list of nodes in the graph
+ * For N total nodes, returns data in the following format:
+ * The node IDs and type IDs are consecutive shorts
+ * The node names are null-separated
+ * |---------------------------------------------------------------|
+ * |  data index ||   0 - 2*N-1  |  2*N - 4*N-1  | 4*N - (< 4096)  |
+ * |---------------------------------------------------------------|
+ * |   parameter ||  Node IDs   | Node type IDs  |    Node names   |
+ * |---------------------------------------------------------------|
+ * |       bytes ||     2*N     |      2*N       |      < 4096     |
+ * |---------------------------------------------------------------|
+ */
+int cb_getnodes(modular_structs_t* structs, metadata_t *meta,  u8 *data, u16 length) {
+	const struct computation_graph* graph = structs->parameter_struct.graph;
+	if (graph->n_nodes >= 150) {
+		static char* error_msg = "Over 150 nodes. Not responding to cb_getnodes for fear of buffer overflow.";
+		send_data(&structs->hardware_struct.uart, DEBUG_ID, 0,
+				error_msg, sizeof(error_msg));
+		return -1;
+	}
+	
+	// Number of bytes in node ID being sent. Currently short (16 bits)
+	size_t id_len = 2;
+
+	char resp_buf[4096];
+	int i;
+	// Currently ID is always index in array.
+	// computation_graph provides no method of accessing ID, since it is implicit
+	for (i = 0; i < graph->n_nodes; i++) {
+		pack_short(i, resp_buf + (id_len * i));
+	}
+
+	// Construct type IDs
+	size_t offset = id_len * graph->n_nodes;
+	for (i = 0; i < graph->n_nodes; i++) {
+		int type_id = graph->nodes[i].type->type_id;
+		pack_short(type_id, resp_buf + offset + (id_len * i));
+	}
+
+	// Construct list of node names
+	offset += id_len * graph->n_nodes;
+	for (i = 0; i < graph->n_nodes; i++) {
+		size_t remaining_size = sizeof(resp_buf) - offset;
+		const char* name = graph->nodes[i].name;
+		size_t name_len = strlen(name);
+		if (name_len + 1 <= remaining_size) {
+			memcpy(resp_buf + offset, name, name_len);
+			offset += name_len;
+			// Add null-terminator separator
+			resp_buf[offset] = 0;
+			offset += 1;
+		}
+	}
+
+	send_data(&structs->hardware_struct.uart, RESPNODES_ID, meta->msg_id, resp_buf, offset);
+	return 0;
+}
+
+/*
+ * Handles adding a new node with a particular type and name
+ * Expects the uart buff to have data in the following format:
+ * |---------------------------------------------|
+ * |  data index ||    0 - 1     |     2 - ?     |
+ * |---------------------------------------------|
+ * |   parameter ||    type ID   | New node name |
+ * |---------------------------------------------|
+ * |       bytes ||       2      |       ?       |
+ * |---------------------------------------------|
+ *
+ * Returns the new node ID in the following format:
+ * |-----------------------------|
+ * |  data index ||    0 - 1     | 
+ * |-----------------------------|
+ * |   parameter ||    node ID   |
+ * |------------------------------
+ * |       bytes ||       2      |
+ * |-----------------------------|
+ */
+int cb_addnode(modular_structs_t* structs, metadata_t *meta,  u8 *data, u16 length) {
+	if (length < 2) {return -1;}
+	// Size of name
+	size_t name_len = length - 2;
+
+	return 0;
+}
diff --git a/quad/src/quad_app/callbacks.h b/quad/src/quad_app/callbacks.h
index 80d0bbc43a5634cf176d084292ddae0376599773..3a13626dbd13e8c3d5bba48cf6c4675069e96394 100644
--- a/quad/src/quad_app/callbacks.h
+++ b/quad/src/quad_app/callbacks.h
@@ -4,8 +4,6 @@
 /* Grab some stupid stuff from legacy code */
 struct modular_structs;
 struct metadata;
-
-/* Make commands.c happy */
 typedef int (command_cb)(struct modular_structs *, struct metadata *, unsigned char *, unsigned short);
 
 #endif
diff --git a/quad/src/quad_app/commands.c b/quad/src/quad_app/commands.c
deleted file mode 120000
index e9bada254a300676bc4a563b5d1183e02125693f..0000000000000000000000000000000000000000
--- a/quad/src/quad_app/commands.c
+++ /dev/null
@@ -1 +0,0 @@
-../../../groundStation/src/backend/common/commands.c
\ No newline at end of file
diff --git a/quad/src/quad_app/commands.h b/quad/src/quad_app/commands.h
deleted file mode 120000
index 92d6b32d4641fd6b8ea711d7141b7b03d484b72a..0000000000000000000000000000000000000000
--- a/quad/src/quad_app/commands.h
+++ /dev/null
@@ -1 +0,0 @@
-../../../groundStation/src/backend/common/commands.h
\ No newline at end of file
diff --git a/quad/src/quad_app/communication.c b/quad/src/quad_app/communication.c
index 619257cb062e480d32146abaeecceba3021cc1cc..c0d37659e29c0b8145b00e15f9a3302e2ef343ae 100644
--- a/quad/src/quad_app/communication.c
+++ b/quad/src/quad_app/communication.c
@@ -83,11 +83,12 @@ int process_packet(modular_structs_t *structs) {
   meta.msg_type = packet[2] << 8 | packet[1];
   meta.msg_id = packet[4] << 8 | packet[3];
   meta.data_len = packet[6] << 8 | packet[5];
-  unsigned char packet_checksum = packet[7+meta.data_len];
 
   // Call appropriate function for packet
-  (* (MessageTypes[meta.msg_type].functionPtr))(structs,&meta,&packet[PACKET_HEADER_SIZE], meta.data_len);
-
+  command_cb* func_ptr = MessageTypes[meta.msg_type].functionPtr;
+  if (meta.msg_type < MAX_TYPE_ID && func_ptr) {
+	  (* func_ptr)(structs,&meta,&packet[PACKET_HEADER_SIZE], meta.data_len);
+  }
   // Done processing packets, reset state
   bytes_recv = 0;
 
diff --git a/quad/src/quad_app/control_algorithm.c b/quad/src/quad_app/control_algorithm.c
index 5e1ebfd7dd227de54ab2ec80b02896b02d26694b..424f098cbe02be1fb3e7707b6383fa0814c99957 100644
--- a/quad/src/quad_app/control_algorithm.c
+++ b/quad/src/quad_app/control_algorithm.c
@@ -8,11 +8,7 @@
 // This implemented modular quadrotor software implements a PID control algorithm
 
 #include "control_algorithm.h"
-#include "node_pid.h"
-#include "node_bounds.h"
-#include "node_constant.h"
-#include "node_mixer.h"
-#include "node_add.h"
+#include "graph_blocks.h"
 #include "PID.h"
 #include "util.h"
 #include "timer.h"
@@ -43,55 +39,55 @@ int control_algorithm_init(parameter_t * ps)
     ps->graph = graph;
 
     // Create all the PID blocks
-    ps->roll_pid = graph_add_node_pid(graph, "Roll PID");
-    ps->pitch_pid = graph_add_node_pid(graph, "Pitch PID");
-    ps->yaw_pid = graph_add_node_pid(graph, "Yaw PID");
-    ps->roll_r_pid = graph_add_node_pid(graph, "Roll Rate PID");
-    ps->pitch_r_pid = graph_add_node_pid(graph, "Pitch Rate PID");
-    ps->yaw_r_pid = graph_add_node_pid(graph, "Yaw Rate PID");
-    ps->x_pos_pid = graph_add_node_pid(graph, "X pos PID");
-    ps->y_pos_pid = graph_add_node_pid(graph, "Y pos PID");
-    ps->alt_pid = graph_add_node_pid(graph, "Altitude PID");
-    ps->x_set = graph_add_node_const(graph, "X Setpoint"); // ID 9
-    ps->y_set = graph_add_node_const(graph, "Y Setpoint");
-    ps->alt_set = graph_add_node_const(graph, "Alt Setpoint");
-    ps->yaw_set = graph_add_node_const(graph, "Yaw Setpoint");
-    ps->throttle_trim = graph_add_node_const(graph, "Throttle trim");
-    ps->throttle_trim_add = graph_add_node_add(graph, "T trim add");
+    ps->roll_pid = graph_add_defined_block(graph, BLOCK_PID, "Roll PID");
+    ps->pitch_pid = graph_add_defined_block(graph, BLOCK_PID, "Pitch PID");
+    ps->yaw_pid = graph_add_defined_block(graph, BLOCK_PID, "Yaw PID");
+    ps->roll_r_pid = graph_add_defined_block(graph, BLOCK_PID, "Roll Rate PID");
+    ps->pitch_r_pid = graph_add_defined_block(graph, BLOCK_PID, "Pitch Rate PID");
+    ps->yaw_r_pid = graph_add_defined_block(graph, BLOCK_PID, "Yaw Rate PID");
+    ps->x_pos_pid = graph_add_defined_block(graph, BLOCK_PID, "X pos PID");
+    ps->y_pos_pid = graph_add_defined_block(graph, BLOCK_PID, "Y pos PID");
+    ps->alt_pid = graph_add_defined_block(graph, BLOCK_PID, "Altitude PID");
+    ps->x_set = graph_add_defined_block(graph, BLOCK_CONSTANT, "X Setpoint"); // ID 9
+    ps->y_set = graph_add_defined_block(graph, BLOCK_CONSTANT, "Y Setpoint");
+    ps->alt_set = graph_add_defined_block(graph, BLOCK_CONSTANT, "Alt Setpoint");
+    ps->yaw_set = graph_add_defined_block(graph, BLOCK_CONSTANT, "Yaw Setpoint");
+    ps->throttle_trim = graph_add_defined_block(graph, BLOCK_CONSTANT, "Throttle trim");
+    ps->throttle_trim_add = graph_add_defined_block(graph, BLOCK_ADD, "T trim add");
 
     // Create blocks for sensor inputs
-    ps->cur_pitch = graph_add_node_const(graph, "Pitch"); // ID 20
-    ps->cur_roll = graph_add_node_const(graph, "Roll");
-    ps->cur_yaw = graph_add_node_const(graph, "Yaw");
+    ps->cur_pitch = graph_add_defined_block(graph, BLOCK_CONSTANT, "Pitch"); // ID 15
+    ps->cur_roll = graph_add_defined_block(graph, BLOCK_CONSTANT, "Roll");
+    ps->cur_yaw = graph_add_defined_block(graph, BLOCK_CONSTANT, "Yaw");
 	// Yaw angular velocity PID
     // theta_dot is the angular velocity about the y-axis
     // phi_dot is the angular velocity about the x-axis
 	// psi_dot is the angular velocity about the z-axis
 	// These are calculated from using the gimbal equations
-    ps->theta_dot = graph_add_node_const(graph, "dTheta");
-    ps->phi_dot = graph_add_node_const(graph, "dPhi");
-    ps->psi_dot = graph_add_node_const(graph, "dPsi");
-    ps->clamp_d_pwmP = graph_add_node_bounds(graph, "P PWM Clamp");
-    ps->clamp_d_pwmR = graph_add_node_bounds(graph, "R PWM Clamp");
-    ps->clamp_d_pwmY = graph_add_node_bounds(graph, "Y PWM Clamp");
+    ps->theta_dot = graph_add_defined_block(graph, BLOCK_CONSTANT, "dTheta");
+    ps->phi_dot = graph_add_defined_block(graph, BLOCK_CONSTANT, "dPhi");
+    ps->psi_dot = graph_add_defined_block(graph, BLOCK_CONSTANT, "dPsi");
+    ps->clamp_d_pwmP = graph_add_defined_block(graph, BLOCK_BOUNDS, "P PWM Clamp");
+    ps->clamp_d_pwmR = graph_add_defined_block(graph, BLOCK_BOUNDS, "R PWM Clamp");
+    ps->clamp_d_pwmY = graph_add_defined_block(graph, BLOCK_BOUNDS, "Y PWM Clamp");
 
     // Create blocks for VRPN data
-    ps->vrpn_x = graph_add_node_const(graph, "VRPN X");
-    ps->vrpn_y = graph_add_node_const(graph, "VRPN Y");
-    ps->vrpn_alt = graph_add_node_const(graph, "VRPN Alt");
-    ps->vrpn_pitch = graph_add_node_const(graph, "VRPN Pitch");
-    ps->vrpn_roll = graph_add_node_const(graph, "VRPN Roll");
+    ps->vrpn_x = graph_add_defined_block(graph, BLOCK_CONSTANT, "VRPN X");
+    ps->vrpn_y = graph_add_defined_block(graph, BLOCK_CONSTANT, "VRPN Y");
+    ps->vrpn_alt = graph_add_defined_block(graph, BLOCK_CONSTANT, "VRPN Alt");
+    ps->vrpn_pitch = graph_add_defined_block(graph, BLOCK_CONSTANT, "VRPN Pitch");
+    ps->vrpn_roll = graph_add_defined_block(graph, BLOCK_CONSTANT, "VRPN Roll");
 
     // Create blocks for RC controller
-    ps->rc_pitch = graph_add_node_const(graph, "RC Pitch");
-    ps->rc_roll = graph_add_node_const(graph, "RC Roll");
-    ps->rc_yaw = graph_add_node_const(graph, "RC Yaw");
-    ps->rc_throttle = graph_add_node_const(graph, "RC Throttle");
+    ps->rc_pitch = graph_add_defined_block(graph, BLOCK_CONSTANT, "RC Pitch");
+    ps->rc_roll = graph_add_defined_block(graph, BLOCK_CONSTANT, "RC Roll");
+    ps->rc_yaw = graph_add_defined_block(graph, BLOCK_CONSTANT, "RC Yaw");
+    ps->rc_throttle = graph_add_defined_block(graph, BLOCK_CONSTANT, "RC Throttle");
 
-    ps->mixer = graph_add_node_mixer(graph, "Signal Mixer");
+    ps->mixer = graph_add_defined_block(graph, BLOCK_MIXER, "Signal Mixer");
 
-    ps->angle_time = graph_add_node_const(graph, "Ts_IMU");
-    ps->pos_time = graph_add_node_const(graph, "Ts_VRPN");
+    ps->angle_time = graph_add_defined_block(graph, BLOCK_CONSTANT, "Ts_IMU");
+    ps->pos_time = graph_add_defined_block(graph, BLOCK_CONSTANT, "Ts_VRPN");
 
     // Connect pitch PID chain
     graph_set_source(graph, ps->pitch_r_pid, PID_SETPOINT, ps->pitch_pid, PID_CORRECTION);
diff --git a/quad/src/quad_app/log_data.c b/quad/src/quad_app/log_data.c
index d7a361dfe69963339efd502e89cf697446bfe5eb..4ad8410b526249d0499b33095a249183e187df04 100644
--- a/quad/src/quad_app/log_data.c
+++ b/quad/src/quad_app/log_data.c
@@ -14,9 +14,7 @@
 #include "log_data.h"
 #include "communication.h"
 #include "computation_graph.h"
-#include "node_pid.h"
-#include "node_constant.h"
-#include "node_mixer.h"
+#include "graph_blocks.h"
 
 // Current index of the log array
 int arrayIndex = 0;
@@ -187,7 +185,7 @@ void printLogging(hardware_t *hardware_struct, log_t* log_struct, parameter_t* p
 	// Comment header
 	safe_sprintf_cat(&buf, "# MicroCART On-board Quad Log\n# Sample size: %d\n", arrayIndex);
 	// Header names for the pre-defined values
-	safe_sprintf_cat(&buf, "time\taccel_x\taccel_y\taccel_z\tgyro_x\tgyro_y\tgyro_z");
+	safe_sprintf_cat(&buf, "%Time\taccel_x\taccel_y\taccel_z\tgyro_x\tgyro_y\tgyro_z");
 
 	int i;
 	// Print all the recorded block parameters
@@ -209,7 +207,7 @@ void printLogging(hardware_t *hardware_struct, log_t* log_struct, parameter_t* p
 
 	// Send units header
 	buf.size = 0;
-	safe_sprintf_cat(&buf, "s\tG\tG\tG\trad/s\trad/s\trad/s"); // The pre-defined ones
+	safe_sprintf_cat(&buf, "&s\tG\tG\tG\trad/s\trad/s\trad/s"); // The pre-defined ones
 	safe_sprintf_cat(&buf, units_output.str);
 	safe_sprintf_cat(&buf, units_param.str);
 	safe_sprintf_cat(&buf, "\n");
diff --git a/quad/src/quad_app/util.c b/quad/src/quad_app/util.c
index 25a8dae95cce20ef69fd5d3e71302269aac1b513..c021fef37cc1daeb6adafe20a75b25e1ec99e558 100644
--- a/quad/src/quad_app/util.c
+++ b/quad/src/quad_app/util.c
@@ -93,3 +93,16 @@ float build_float(u8 *buff) {
     | buff[0];
   return x.f;
 }
+
+int16_t build_short(u8* buff) {
+	return (buff[0] & 0xFF) | ((buff[1] & 0xFF) << 8);
+}
+
+void pack_short(int16_t val, u8* buff) {
+	buff[0] = val & 0xFF;
+	buff[1] = (val >> 8) & 0xFF;
+}
+
+void pack_float(float val, u8* buff) {
+	memcpy(buff, &val, sizeof(val));
+}
diff --git a/quad/src/quad_app/util.h b/quad/src/quad_app/util.h
index 0d06e5fe0075b5c0808a2bca4e8746e57ecf7dcf..643498afc53753720c8f4295fc88918ad5eae4db 100644
--- a/quad/src/quad_app/util.h
+++ b/quad/src/quad_app/util.h
@@ -18,5 +18,9 @@ void kill_motors(struct PWMOutputDriver *pwm_outputs);
 
 int build_int(u8 *buff);
 float build_float(u8 *buff);
+int16_t build_short(u8* buff);
+
+void pack_short(int16_t val, u8* buff);
+void pack_float(float val, u8* buff);
 
 #endif //_UTIL_H
diff --git a/quad/src/virt_quad/Makefile b/quad/src/virt_quad/Makefile
new file mode 100644
index 0000000000000000000000000000000000000000..3626790b07efec7ea75a039491b6078a8c6918df
--- /dev/null
+++ b/quad/src/virt_quad/Makefile
@@ -0,0 +1,6 @@
+TOP=../..
+
+NAME = virt_quad
+REQLIBS = -lquad_app -lcomputation_graph -lm -lcommands -lgraph_blocks
+
+include $(TOP)/executable.mk
diff --git a/quad/src/virt_quad/hw_impl_unix.c b/quad/src/virt_quad/hw_impl_unix.c
new file mode 100644
index 0000000000000000000000000000000000000000..fea231c35e963f5321b35665e4c3c2feac961383
--- /dev/null
+++ b/quad/src/virt_quad/hw_impl_unix.c
@@ -0,0 +1,70 @@
+#include "hw_impl_unix.h"
+
+struct UARTDriver create_unix_uart() {
+  struct UARTDriver uart;
+  uart.state = NULL;
+  uart.reset = unix_uart_reset;
+  uart.write = unix_uart_write;
+  uart.read = unix_uart_read;
+  return uart;
+}
+
+struct PWMOutputDriver create_unix_pwm_outputs() {
+  struct PWMOutputDriver pwm_outputs;
+  pwm_outputs.state = NULL;
+  pwm_outputs.reset = unix_pwm_output_reset;
+  pwm_outputs.write = unix_pwm_output_write;
+  return pwm_outputs;
+}
+
+struct PWMInputDriver create_unix_pwm_inputs() {
+  struct PWMInputDriver pwm_inputs;
+  pwm_inputs.state = NULL;
+  pwm_inputs.reset = unix_pwm_input_reset;
+  pwm_inputs.read = unix_pwm_input_read;
+  return pwm_inputs;
+}
+
+struct I2CDriver create_unix_i2c() {
+  struct I2CDriver i2c;
+  i2c.state = NULL;
+  i2c.reset = unix_i2c_reset;
+  i2c.write = unix_i2c_write;
+  i2c.read = unix_i2c_read;
+  return i2c;
+}
+
+struct TimerDriver create_unix_global_timer() {
+  struct TimerDriver global_timer;
+  global_timer.state = NULL;
+  global_timer.reset = unix_global_timer_reset;
+  global_timer.restart = unix_global_timer_restart;
+  global_timer.read = unix_global_timer_read;
+  return global_timer;
+}
+
+struct TimerDriver create_unix_axi_timer() {
+  struct TimerDriver axi_timer;
+  axi_timer.state = NULL;
+  axi_timer.reset = unix_axi_timer_reset;
+  axi_timer.restart = unix_axi_timer_restart;
+  axi_timer.read = unix_axi_timer_read;
+  return axi_timer;
+}
+
+struct LEDDriver create_unix_mio7_led() {
+  struct LEDDriver mio7_led;
+  mio7_led.state = NULL;
+  mio7_led.reset = unix_mio7_led_reset;
+  mio7_led.turn_on = unix_mio7_led_turn_on;
+  mio7_led.turn_off = unix_mio7_led_turn_off;
+  return mio7_led;
+}
+
+struct SystemDriver create_unix_system() {
+  struct SystemDriver sys;
+  sys.state = NULL;
+  sys.reset = unix_system_reset;
+  sys.sleep = unix_system_sleep;
+  return sys;
+}
diff --git a/quad/src/virt_quad/hw_impl_unix.h b/quad/src/virt_quad/hw_impl_unix.h
new file mode 100644
index 0000000000000000000000000000000000000000..c7e3659dffcfb0b1094c26f41ffe3b4d792962b8
--- /dev/null
+++ b/quad/src/virt_quad/hw_impl_unix.h
@@ -0,0 +1,61 @@
+#ifndef HW_IMPL_UNIX
+#define HW_IMPL_UNIX
+
+#include "hw_iface.h"
+#include "type_def.h"
+
+#include <sys/time.h>
+#include <stdio.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <time.h>
+
+int unix_uart_reset(struct UARTDriver *self);
+int unix_uart_write(struct UARTDriver *self, unsigned char c);
+int unix_uart_read(struct UARTDriver *self, unsigned char *c);
+
+int unix_pwm_output_reset(struct PWMOutputDriver *self);
+int unix_pwm_output_write(struct PWMOutputDriver *self, unsigned int channel, unsigned long pulse_width_us);
+
+int unix_pwm_input_reset(struct PWMInputDriver *self);
+int unix_pwm_input_read(struct PWMInputDriver *self, unsigned int channel, unsigned long *pulse_width_us);
+
+int unix_i2c_reset(struct I2CDriver *self);
+int unix_i2c_write(struct I2CDriver *self,
+                   unsigned short device_addr,
+                   unsigned char *data,
+                   unsigned int length);
+int unix_i2c_read(struct I2CDriver *self,
+                  unsigned short device_addr,
+                  unsigned char *buff,
+                  unsigned int length);
+
+int unix_global_timer_reset(struct TimerDriver *self);
+int unix_global_timer_restart(struct TimerDriver *self);
+int unix_global_timer_read(struct TimerDriver *self, u64 *us);
+
+int unix_axi_timer_reset(struct TimerDriver *self);
+int unix_axi_timer_restart(struct TimerDriver *self);
+int unix_axi_timer_read(struct TimerDriver *self, u64 *us);
+
+int unix_mio7_led_reset(struct LEDDriver *self);
+int unix_mio7_led_turn_on(struct LEDDriver *self);
+int unix_mio7_led_turn_off(struct LEDDriver *self);
+
+int unix_system_reset(struct SystemDriver *self);
+int unix_system_sleep(struct SystemDriver *self, unsigned long us);
+
+struct UARTDriver create_unix_uart();
+struct PWMOutputDriver create_unix_pwm_outputs();
+struct PWMInputDriver create_unix_pwm_inputs();
+struct I2CDriver create_unix_i2c();
+struct TimerDriver create_unix_global_timer();
+struct TimerDriver create_unix_axi_timer();
+struct LEDDriver create_unix_mio7_led();
+struct SystemDriver create_unix_system();
+
+int test_unix_i2c();
+int test_unix_mio7_led_and_system();
+int test_unix_pwm_inputs();
+
+#endif
diff --git a/quad/src/virt_quad/hw_impl_unix_axi_timer.c b/quad/src/virt_quad/hw_impl_unix_axi_timer.c
new file mode 100644
index 0000000000000000000000000000000000000000..73e9acc281110e6453bfba625976af852eeac351
--- /dev/null
+++ b/quad/src/virt_quad/hw_impl_unix_axi_timer.c
@@ -0,0 +1,24 @@
+#include "hw_impl_unix.h"
+
+int unix_axi_timer_reset(struct TimerDriver *self) {
+  if (self->state == NULL) {
+    self->state = malloc(sizeof(struct timeval));
+  }
+  return 0;
+}
+
+int unix_axi_timer_restart(struct TimerDriver *self) {
+  struct timeval *start = self->state;
+  struct timezone tz;
+  gettimeofday(start, &tz);
+  return 0;
+}
+
+int unix_axi_timer_read(struct TimerDriver *self, u64 *us) {
+  struct timeval *start = self->state;
+  struct timeval end;
+  struct timezone tz;
+  gettimeofday(&end, &tz);
+  *us = end.tv_usec - start->tv_usec;
+  return 0;
+}
diff --git a/quad/src/virt_quad/hw_impl_unix_global_timer.c b/quad/src/virt_quad/hw_impl_unix_global_timer.c
new file mode 100644
index 0000000000000000000000000000000000000000..be752b774eb55cb17f3c481947f44b76fc2ec124
--- /dev/null
+++ b/quad/src/virt_quad/hw_impl_unix_global_timer.c
@@ -0,0 +1,24 @@
+#include "hw_impl_unix.h"
+
+int unix_global_timer_reset(struct TimerDriver *self) {
+  if (self->state == NULL) {
+    self->state = malloc(sizeof(struct timeval));
+  }
+  return 0;
+}
+
+int unix_global_timer_restart(struct TimerDriver *self) {
+  struct timeval *start = self->state;
+  struct timezone tz;
+  gettimeofday(start, &tz);
+  return 0;
+}
+
+int unix_global_timer_read(struct TimerDriver *self, u64 *us) {
+  struct timeval *start = self->state;
+  struct timeval end;
+  struct timezone tz;
+  gettimeofday(&end, &tz);
+  *us = end.tv_usec - start->tv_usec;
+  return 0;
+}
diff --git a/quad/src/virt_quad/hw_impl_unix_i2c.c b/quad/src/virt_quad/hw_impl_unix_i2c.c
new file mode 100644
index 0000000000000000000000000000000000000000..09ab43c8b83b82d60dff7623a31efb1d29c3fd60
--- /dev/null
+++ b/quad/src/virt_quad/hw_impl_unix_i2c.c
@@ -0,0 +1,19 @@
+#include "hw_impl_unix.h"
+
+int unix_i2c_reset(struct I2CDriver *self) {
+  return 0;
+}
+
+int unix_i2c_write(struct I2CDriver *self,
+                   unsigned short device_addr,
+                   unsigned char *data,
+                   unsigned int length) {
+  return 0;
+}
+
+int unix_i2c_read(struct I2CDriver *self,
+                  unsigned short device_addr,
+                  unsigned char *buff,
+                  unsigned int length) {
+  return 0;
+}
diff --git a/quad/src/virt_quad/hw_impl_unix_mio7_led.c b/quad/src/virt_quad/hw_impl_unix_mio7_led.c
new file mode 100644
index 0000000000000000000000000000000000000000..f96fda029e0c0a9664be8bd81e76a93af606e7ba
--- /dev/null
+++ b/quad/src/virt_quad/hw_impl_unix_mio7_led.c
@@ -0,0 +1,23 @@
+#include "hw_impl_unix.h"
+
+int on;
+
+int unix_mio7_led_reset(struct LEDDriver *self) {
+  return 0;
+}
+
+int unix_mio7_led_turn_on(struct LEDDriver *self) {
+  if (!on) {
+    puts("LED ON");
+    on = 1;
+  }
+  return 0;
+}
+
+int unix_mio7_led_turn_off(struct LEDDriver *self) {
+  if (on) {
+    puts("LED OFF");
+    on = 0;
+  }
+  return 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
new file mode 100644
index 0000000000000000000000000000000000000000..8881f5f3815f063fb2ce365a6b07c79d7a2eaf44
--- /dev/null
+++ b/quad/src/virt_quad/hw_impl_unix_pwm_input.c
@@ -0,0 +1,50 @@
+#include "hw_impl_unix.h"
+
+int unix_pwm_input_reset(struct PWMInputDriver *self) {
+  return 0;
+}
+
+int unix_pwm_input_read(struct PWMInputDriver *self,
+                        unsigned int channel,
+                        unsigned long *pulse_width_us) {
+  static int inc = 0;
+  unsigned long gear;
+
+  switch (channel) {
+  case 0:
+    *pulse_width_us = 100000;
+    break;
+  case 1:
+    *pulse_width_us = 100000;
+    break;
+  case 2:
+    *pulse_width_us = 100000;
+    break;
+  case 3:
+    *pulse_width_us = 100000;
+    break;
+  case 4:
+    if (inc == 0) {
+      inc += 1;
+      puts("GEAR OFF");
+    }
+    if (inc < 20) {
+      inc += 1;
+      gear = 120000;
+    }
+    else if (inc == 20) {
+      puts("GEAR ON");
+      inc += 1;
+    }
+    else {
+      gear = 140000;
+    }
+    *pulse_width_us = gear;
+    break;
+  case 5:
+    // flap 1
+    *pulse_width_us = 192000;
+    break;
+  }
+  return 0;
+}
diff --git a/quad/src/virt_quad/hw_impl_unix_pwm_output.c b/quad/src/virt_quad/hw_impl_unix_pwm_output.c
new file mode 100644
index 0000000000000000000000000000000000000000..b549cd4367af0a5ebfe8a50cd7af97e94daf16bb
--- /dev/null
+++ b/quad/src/virt_quad/hw_impl_unix_pwm_output.c
@@ -0,0 +1,12 @@
+#include "hw_impl_unix.h"
+
+int unix_pwm_output_reset(struct PWMOutputDriver *self) {
+  return 0;
+}
+
+int unix_pwm_output_write(struct PWMOutputDriver *self,
+                          unsigned int channel,
+                          unsigned long pulse_width_us) {
+  //printf("PWM OUTPUT: %d %d\n", channel, pulse_width_us);
+  return 0;
+}
diff --git a/quad/src/virt_quad/hw_impl_unix_system.c b/quad/src/virt_quad/hw_impl_unix_system.c
new file mode 100644
index 0000000000000000000000000000000000000000..27f1a691ee92f46f6be0e029245dfba51bfb9de0
--- /dev/null
+++ b/quad/src/virt_quad/hw_impl_unix_system.c
@@ -0,0 +1,14 @@
+#include "hw_impl_unix.h"
+
+int unix_system_reset(struct SystemDriver *sys) {
+  return 0;
+}
+
+int unix_system_sleep(struct SystemDriver *sys, unsigned long us) {
+  struct timespec time;
+  struct timespec time2;
+  time.tv_sec = 0;
+  time.tv_nsec = us * 1000;
+  nanosleep(&time, &time2);
+  return 0;
+}
diff --git a/quad/src/virt_quad/hw_impl_unix_uart.c b/quad/src/virt_quad/hw_impl_unix_uart.c
new file mode 100644
index 0000000000000000000000000000000000000000..c4f024ff282e6038f4ef63bd88a615fc14e9673f
--- /dev/null
+++ b/quad/src/virt_quad/hw_impl_unix_uart.c
@@ -0,0 +1,70 @@
+#include "hw_impl_unix.h"
+#include <sys/types.h>       
+#include <sys/socket.h>
+#include <stdio.h>
+#include <sys/un.h>
+#include <sys/ioctl.h>
+#include <err.h>
+#include <netinet/in.h>
+
+#define DEFAULT_SOCKET "../../groundStation/virtquad.socket"
+#define SOCKET_ENV "VIRT_QUAD_SOCKET"
+
+static int backendSocket;
+static int client;
+
+int unix_uart_reset(struct UARTDriver *self) {
+	char * backend_socket_path = DEFAULT_SOCKET;
+	if (getenv(SOCKET_ENV)) {
+		backend_socket_path = getenv(SOCKET_ENV);
+	}
+
+	/* Unlink if it exists */
+	unlink(backend_socket_path);
+	printf("using socket '%s'\n", backend_socket_path);
+
+	/* Create socket */
+	backendSocket = socket(AF_UNIX, SOCK_STREAM | SOCK_NONBLOCK, 0);
+
+
+	/* Create sockaddr and bind */
+	struct sockaddr_un sa;
+	sa.sun_family = AF_UNIX;
+	strncpy(sa.sun_path, backend_socket_path, strlen(backend_socket_path));
+	sa.sun_path[strlen(backend_socket_path)+1] = '\0';
+	if (bind(backendSocket, (struct sockaddr *) &sa, sizeof(sa))) {
+		err(-1, "bind");
+	}
+
+	/* Listen */
+	if (listen(backendSocket, 1)) {
+		err(-1, "listen");
+	}
+
+	while (1) {
+		client = accept(backendSocket, NULL, NULL);
+		if (client > 0)
+			break;
+	}
+	printf(" accpet() returned with %d\n", client);
+  return 0;
+}
+
+int unix_uart_write(struct UARTDriver *self, unsigned char c) {
+	return send(client, &c, 1, MSG_DONTWAIT);
+	// return write(client, &c, 1);
+}
+
+int unix_uart_read(struct UARTDriver *self, unsigned char *c) {
+	int bytes_available;
+	ioctl(client,FIONREAD,&bytes_available);
+	
+	if (bytes_available > 0) {
+		int bytes = recv(client, c, 1, 0);
+		printf("read in %d byte [%x]\n", bytes, *c);
+		return bytes;
+	} else {
+		return 0;
+	}
+	
+}
\ No newline at end of file
diff --git a/quad/src/virt_quad/main.c b/quad/src/virt_quad/main.c
new file mode 100644
index 0000000000000000000000000000000000000000..a87b8013f5037d6a2db5badd74c7166f681e3e8c
--- /dev/null
+++ b/quad/src/virt_quad/main.c
@@ -0,0 +1,21 @@
+#include <stdio.h>
+#include "hw_impl_unix.h"
+#include "quad_app.h"
+
+int setup_hardware(hardware_t *hardware) {
+  hardware->i2c = create_unix_i2c();
+  hardware->pwm_inputs = create_unix_pwm_inputs();
+  hardware->pwm_outputs = create_unix_pwm_outputs();
+  hardware->uart = create_unix_uart();
+  hardware->global_timer = create_unix_global_timer();
+  hardware->axi_timer = create_unix_axi_timer();
+  hardware->mio7_led = create_unix_mio7_led();
+  hardware->sys = create_unix_system();
+  return 0;
+}
+
+int main()
+{
+  quad_main(setup_hardware);
+  return 0;
+}
diff --git a/quad/test_logging/Makefile b/quad/test_logging/Makefile
new file mode 100644
index 0000000000000000000000000000000000000000..d30eb0102cea878e910d440437f4c185e2562b30
--- /dev/null
+++ b/quad/test_logging/Makefile
@@ -0,0 +1,3 @@
+CC=gcc
+test: test.c
+	$(CC) -o test_log -I. -I../src/quad_app -I../src/computation_graph test.c ../src/quad_app/*.c ../src/computation_graph/*.c -lm
diff --git a/quad/test_logging/log.txt b/quad/test_logging/log.txt
new file mode 100644
index 0000000000000000000000000000000000000000..c66e5152c66693e82bc287674ba3359ab6710dd4
--- /dev/null
+++ b/quad/test_logging/log.txt
@@ -0,0 +1,7 @@
+# MicroCART On-board Quad Log
+# Sample size: 3
+time	accel_x	accel_y	accel_z	gyro_x	gyro_y	gyro_z	Altitude PID_Correction	X pos PID_Correction	Y pos PID_Correction	Pitch PID_Correction	Roll PID_Correction	Yaw PID_Correction	Pitch Rate PID_Correction	Roll Rate PID_Correction	Yaw Rate PID_Correction	Pitch_Constant	Roll_Constant	Yaw_Constant	VRPN X_Constant	VRPN Y_Constant	VRPN Alt_Constant	VRPN Pitch_Constant	VRPN Roll_Constant	X Setpoint_Constant	Y Setpoint_Constant	Alt Setpoint_Constant	Yaw Setpoint_Constant	Signal Mixer_PWM 0	Signal Mixer_PWM 1	Signal Mixer_PWM 2	Signal Mixer_PWM 3
+s	G	G	G	rad/s	rad/s	rad/s	10ns_dutycycle	rad	rad	rad/s	rad/s	rad/s	10ns_dutycycle	10ns_dutycycle	10ns_dutycycle	rad	rad	rad	m	m	m	rad	rad	m	m	m	rad	10ns_dutycycle	10ns_dutycycle	10ns_dutycycle	10ns_dutycycle
+0.000000	0.000000	0.000000	0.000000	0.000000	4722366482869645213696.000000	-nan	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000
+0.000000	0.000000	0.000000	0.000000	0.000000	4722366482869645213696.000000	-nan	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000
+0.000000	0.000000	0.000000	0.000000	0.000000	4722366482869645213696.000000	-nan	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000
diff --git a/quad/test_logging/test.c b/quad/test_logging/test.c
new file mode 100644
index 0000000000000000000000000000000000000000..7fd26a97d187f96630e209fa04f5364fdbdcef58
--- /dev/null
+++ b/quad/test_logging/test.c
@@ -0,0 +1,22 @@
+#include <stddef.h>
+#include "type_def.h"
+#include "log_data.h"
+#include "control_algorithm.h"
+
+int zybo_uart_write(struct UARTDriver *self, unsigned char c) {
+    printf("%c", c);
+    return 0;
+}
+
+
+int main() {
+    modular_structs_t structs;
+    structs.hardware_struct.uart.write = zybo_uart_write;
+    control_algorithm_init(&(structs.parameter_struct));
+	initialize_logging(&structs.log_struct, &structs.parameter_struct);
+    log_data(&(structs.log_struct), &(structs.parameter_struct));
+    log_data(&(structs.log_struct), &(structs.parameter_struct));
+    log_data(&(structs.log_struct), &(structs.parameter_struct));
+    printLogging(&structs.hardware_struct, &(structs.log_struct), &(structs.parameter_struct));
+    return 0;
+}
diff --git a/quad/test_logging/test_log b/quad/test_logging/test_log
new file mode 100644
index 0000000000000000000000000000000000000000..e9c0b45a27eb1eb96d051a7ec6738d3c8ff5adba
Binary files /dev/null and b/quad/test_logging/test_log differ
diff --git a/quad/xsdk_workspace/modular_quad_pid/.cproject b/quad/xsdk_workspace/modular_quad_pid/.cproject
index b6594e32f340de9327b789312fb975bd37009524..f17eeafddf9cc8821dcdd8ee953460b8a2f47b55 100644
--- a/quad/xsdk_workspace/modular_quad_pid/.cproject
+++ b/quad/xsdk_workspace/modular_quad_pid/.cproject
@@ -27,15 +27,15 @@
 							<tool id="xilinx.gnu.arm.c.toolchain.compiler.debug.177835003" name="ARM gcc compiler" superClass="xilinx.gnu.arm.c.toolchain.compiler.debug">
 								<option defaultValue="gnu.c.optimization.level.none" id="xilinx.gnu.compiler.option.optimization.level.1900496019" name="Optimization Level" superClass="xilinx.gnu.compiler.option.optimization.level" value="gnu.c.optimization.level.none" valueType="enumerated"/>
 								<option id="xilinx.gnu.compiler.option.debugging.level.1207856754" name="Debug Level" superClass="xilinx.gnu.compiler.option.debugging.level" value="gnu.c.debugging.level.max" valueType="enumerated"/>
-								<option id="xilinx.gnu.compiler.inferred.swplatform.includes.2123463819" name="Software Platform Include Path" superClass="xilinx.gnu.compiler.inferred.swplatform.includes" valueType="includePath">
-									<listOptionValue builtIn="false" value="../../system_bsp/ps7_cortexa9_0/include"/>
-								</option>
+								<option id="xilinx.gnu.compiler.inferred.swplatform.includes.2123463819" name="Software Platform Include Path" superClass="xilinx.gnu.compiler.inferred.swplatform.includes" valueType="includePath"/>
 								<option id="xilinx.gnu.compiler.symbols.defined.1696008720" name="Defined symbols (-D)" superClass="xilinx.gnu.compiler.symbols.defined"/>
 								<option id="xilinx.gnu.compiler.dircategory.includes.1211006365" name="Include Paths" superClass="xilinx.gnu.compiler.dircategory.includes" valueType="includePath">
 									<listOptionValue builtIn="false" value="../../system_bsp/ps7_cortexa9_0/include"/>
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/modular_quad_pid/ext/computation_graph}&quot;"/>
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/modular_quad_pid/ext/quad_app}&quot;"/>
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/modular_quad_pid/ext/queue}&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/modular_quad_pid/ext/commands}&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/modular_quad_pid/ext/graph_blocks}&quot;"/>
 								</option>
 								<inputType id="xilinx.gnu.arm.c.compiler.input.909725989" name="C source files" superClass="xilinx.gnu.arm.c.compiler.input"/>
 							</tool>
@@ -108,9 +108,7 @@
 							<tool id="xilinx.gnu.arm.c.toolchain.compiler.release.85270120" name="ARM gcc compiler" superClass="xilinx.gnu.arm.c.toolchain.compiler.release">
 								<option defaultValue="gnu.c.optimization.level.more" id="xilinx.gnu.compiler.option.optimization.level.515686013" name="Optimization Level" superClass="xilinx.gnu.compiler.option.optimization.level" valueType="enumerated"/>
 								<option id="xilinx.gnu.compiler.option.debugging.level.1121150517" name="Debug Level" superClass="xilinx.gnu.compiler.option.debugging.level" value="gnu.c.debugging.level.none" valueType="enumerated"/>
-								<option id="xilinx.gnu.compiler.inferred.swplatform.includes.687694973" name="Software Platform Include Path" superClass="xilinx.gnu.compiler.inferred.swplatform.includes" valueType="includePath">
-									<listOptionValue builtIn="false" value="../../system_bsp/ps7_cortexa9_0/include"/>
-								</option>
+								<option id="xilinx.gnu.compiler.inferred.swplatform.includes.687694973" name="Software Platform Include Path" superClass="xilinx.gnu.compiler.inferred.swplatform.includes"/>
 								<option id="xilinx.gnu.compiler.symbols.defined.1562495938" name="Defined symbols (-D)" superClass="xilinx.gnu.compiler.symbols.defined" valueType="definedSymbols">
 									<listOptionValue builtIn="false" value="NDEBUG=1"/>
 								</option>
@@ -119,6 +117,8 @@
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/modular_quad_pid/ext/computation_graph}&quot;"/>
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/modular_quad_pid/ext/quad_app}&quot;"/>
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/modular_quad_pid/ext/queue}&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/modular_quad_pid/ext/commands}&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/modular_quad_pid/ext/graph_blocks}&quot;"/>
 								</option>
 								<inputType id="xilinx.gnu.arm.c.compiler.input.846429887" name="C source files" superClass="xilinx.gnu.arm.c.compiler.input"/>
 							</tool>
diff --git a/quad/xsdk_workspace/modular_quad_pid/.project b/quad/xsdk_workspace/modular_quad_pid/.project
index 7ae4ca25c0f0cd671784aad1590a7cc52299f7f1..734130fb55d695ee28de0b41cc6b8f0f4bccd05e 100644
--- a/quad/xsdk_workspace/modular_quad_pid/.project
+++ b/quad/xsdk_workspace/modular_quad_pid/.project
@@ -25,11 +25,21 @@
 		<nature>org.eclipse.cdt.managedbuilder.core.ScannerConfigNature</nature>
 	</natures>
 	<linkedResources>
+		<link>
+			<name>ext/commands</name>
+			<type>2</type>
+			<location>/local/ucart/MicroCART_17-18/quad/src/commands</location>
+		</link>
 		<link>
 			<name>ext/computation_graph</name>
 			<type>2</type>
 			<locationURI>QUAD_LOC/src/computation_graph</locationURI>
 		</link>
+		<link>
+			<name>ext/graph_blocks</name>
+			<type>2</type>
+			<location>/local/ucart/MicroCART_17-18/quad/src/graph_blocks</location>
+		</link>
 		<link>
 			<name>ext/quad_app</name>
 			<type>2</type>
diff --git a/quad/xsdk_workspace/modular_quad_pid/Debug/ext/commands/subdir.mk b/quad/xsdk_workspace/modular_quad_pid/Debug/ext/commands/subdir.mk
new file mode 100644
index 0000000000000000000000000000000000000000..f7ba0a3012665b52d4f33945aeb8058b155fa295
--- /dev/null
+++ b/quad/xsdk_workspace/modular_quad_pid/Debug/ext/commands/subdir.mk
@@ -0,0 +1,24 @@
+################################################################################
+# Automatically-generated file. Do not edit!
+################################################################################
+
+# Add inputs and outputs from these tool invocations to the build variables 
+C_SRCS += \
+/local/ucart/MicroCART_17-18/quad/src/commands/commands.c 
+
+OBJS += \
+./ext/commands/commands.o 
+
+C_DEPS += \
+./ext/commands/commands.d 
+
+
+# Each subdirectory must supply rules for building sources it contributes
+ext/commands/commands.o: /local/ucart/MicroCART_17-18/quad/src/commands/commands.c
+	@echo 'Building file: $<'
+	@echo 'Invoking: ARM gcc compiler'
+	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	@echo 'Finished building: $<'
+	@echo ' '
+
+
diff --git a/quad/xsdk_workspace/modular_quad_pid/Debug/ext/computation_graph/subdir.mk b/quad/xsdk_workspace/modular_quad_pid/Debug/ext/computation_graph/subdir.mk
index 5ca516e5ff888247e3f9eb3163662686f2e09f4f..6e59c9cee0577a04e7ea23b17ff244588d9e762d 100644
--- a/quad/xsdk_workspace/modular_quad_pid/Debug/ext/computation_graph/subdir.mk
+++ b/quad/xsdk_workspace/modular_quad_pid/Debug/ext/computation_graph/subdir.mk
@@ -4,80 +4,20 @@
 
 # Add inputs and outputs from these tool invocations to the build variables 
 C_SRCS += \
-/local/ucart/MicroCART_17-18/quad/src/computation_graph/computation_graph.c \
-/local/ucart/MicroCART_17-18/quad/src/computation_graph/node_accumulator.c \
-/local/ucart/MicroCART_17-18/quad/src/computation_graph/node_add.c \
-/local/ucart/MicroCART_17-18/quad/src/computation_graph/node_constant.c \
-/local/ucart/MicroCART_17-18/quad/src/computation_graph/node_gain.c \
-/local/ucart/MicroCART_17-18/quad/src/computation_graph/node_mult.c \
-/local/ucart/MicroCART_17-18/quad/src/computation_graph/node_pow.c 
+/local/ucart/MicroCART_17-18/quad/src/computation_graph/computation_graph.c 
 
 OBJS += \
-./ext/computation_graph/computation_graph.o \
-./ext/computation_graph/node_accumulator.o \
-./ext/computation_graph/node_add.o \
-./ext/computation_graph/node_constant.o \
-./ext/computation_graph/node_gain.o \
-./ext/computation_graph/node_mult.o \
-./ext/computation_graph/node_pow.o 
+./ext/computation_graph/computation_graph.o 
 
 C_DEPS += \
-./ext/computation_graph/computation_graph.d \
-./ext/computation_graph/node_accumulator.d \
-./ext/computation_graph/node_add.d \
-./ext/computation_graph/node_constant.d \
-./ext/computation_graph/node_gain.d \
-./ext/computation_graph/node_mult.d \
-./ext/computation_graph/node_pow.d 
+./ext/computation_graph/computation_graph.d 
 
 
 # Each subdirectory must supply rules for building sources it contributes
 ext/computation_graph/computation_graph.o: /local/ucart/MicroCART_17-18/quad/src/computation_graph/computation_graph.c
 	@echo 'Building file: $<'
 	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
-	@echo 'Finished building: $<'
-	@echo ' '
-
-ext/computation_graph/node_accumulator.o: /local/ucart/MicroCART_17-18/quad/src/computation_graph/node_accumulator.c
-	@echo 'Building file: $<'
-	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
-	@echo 'Finished building: $<'
-	@echo ' '
-
-ext/computation_graph/node_add.o: /local/ucart/MicroCART_17-18/quad/src/computation_graph/node_add.c
-	@echo 'Building file: $<'
-	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
-	@echo 'Finished building: $<'
-	@echo ' '
-
-ext/computation_graph/node_constant.o: /local/ucart/MicroCART_17-18/quad/src/computation_graph/node_constant.c
-	@echo 'Building file: $<'
-	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
-	@echo 'Finished building: $<'
-	@echo ' '
-
-ext/computation_graph/node_gain.o: /local/ucart/MicroCART_17-18/quad/src/computation_graph/node_gain.c
-	@echo 'Building file: $<'
-	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
-	@echo 'Finished building: $<'
-	@echo ' '
-
-ext/computation_graph/node_mult.o: /local/ucart/MicroCART_17-18/quad/src/computation_graph/node_mult.c
-	@echo 'Building file: $<'
-	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
-	@echo 'Finished building: $<'
-	@echo ' '
-
-ext/computation_graph/node_pow.o: /local/ucart/MicroCART_17-18/quad/src/computation_graph/node_pow.c
-	@echo 'Building file: $<'
-	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
 	@echo 'Finished building: $<'
 	@echo ' '
 
diff --git a/quad/xsdk_workspace/modular_quad_pid/Debug/ext/graph_blocks/subdir.mk b/quad/xsdk_workspace/modular_quad_pid/Debug/ext/graph_blocks/subdir.mk
new file mode 100644
index 0000000000000000000000000000000000000000..2bb4dfce904ba88353ba7f556474171b44d79857
--- /dev/null
+++ b/quad/xsdk_workspace/modular_quad_pid/Debug/ext/graph_blocks/subdir.mk
@@ -0,0 +1,114 @@
+################################################################################
+# Automatically-generated file. Do not edit!
+################################################################################
+
+# Add inputs and outputs from these tool invocations to the build variables 
+C_SRCS += \
+/local/ucart/MicroCART_17-18/quad/src/graph_blocks/graph_blocks.c \
+/local/ucart/MicroCART_17-18/quad/src/graph_blocks/node_accumulator.c \
+/local/ucart/MicroCART_17-18/quad/src/graph_blocks/node_add.c \
+/local/ucart/MicroCART_17-18/quad/src/graph_blocks/node_bounds.c \
+/local/ucart/MicroCART_17-18/quad/src/graph_blocks/node_constant.c \
+/local/ucart/MicroCART_17-18/quad/src/graph_blocks/node_gain.c \
+/local/ucart/MicroCART_17-18/quad/src/graph_blocks/node_mixer.c \
+/local/ucart/MicroCART_17-18/quad/src/graph_blocks/node_mult.c \
+/local/ucart/MicroCART_17-18/quad/src/graph_blocks/node_pid.c \
+/local/ucart/MicroCART_17-18/quad/src/graph_blocks/node_pow.c 
+
+OBJS += \
+./ext/graph_blocks/graph_blocks.o \
+./ext/graph_blocks/node_accumulator.o \
+./ext/graph_blocks/node_add.o \
+./ext/graph_blocks/node_bounds.o \
+./ext/graph_blocks/node_constant.o \
+./ext/graph_blocks/node_gain.o \
+./ext/graph_blocks/node_mixer.o \
+./ext/graph_blocks/node_mult.o \
+./ext/graph_blocks/node_pid.o \
+./ext/graph_blocks/node_pow.o 
+
+C_DEPS += \
+./ext/graph_blocks/graph_blocks.d \
+./ext/graph_blocks/node_accumulator.d \
+./ext/graph_blocks/node_add.d \
+./ext/graph_blocks/node_bounds.d \
+./ext/graph_blocks/node_constant.d \
+./ext/graph_blocks/node_gain.d \
+./ext/graph_blocks/node_mixer.d \
+./ext/graph_blocks/node_mult.d \
+./ext/graph_blocks/node_pid.d \
+./ext/graph_blocks/node_pow.d 
+
+
+# Each subdirectory must supply rules for building sources it contributes
+ext/graph_blocks/graph_blocks.o: /local/ucart/MicroCART_17-18/quad/src/graph_blocks/graph_blocks.c
+	@echo 'Building file: $<'
+	@echo 'Invoking: ARM gcc compiler'
+	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	@echo 'Finished building: $<'
+	@echo ' '
+
+ext/graph_blocks/node_accumulator.o: /local/ucart/MicroCART_17-18/quad/src/graph_blocks/node_accumulator.c
+	@echo 'Building file: $<'
+	@echo 'Invoking: ARM gcc compiler'
+	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	@echo 'Finished building: $<'
+	@echo ' '
+
+ext/graph_blocks/node_add.o: /local/ucart/MicroCART_17-18/quad/src/graph_blocks/node_add.c
+	@echo 'Building file: $<'
+	@echo 'Invoking: ARM gcc compiler'
+	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	@echo 'Finished building: $<'
+	@echo ' '
+
+ext/graph_blocks/node_bounds.o: /local/ucart/MicroCART_17-18/quad/src/graph_blocks/node_bounds.c
+	@echo 'Building file: $<'
+	@echo 'Invoking: ARM gcc compiler'
+	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	@echo 'Finished building: $<'
+	@echo ' '
+
+ext/graph_blocks/node_constant.o: /local/ucart/MicroCART_17-18/quad/src/graph_blocks/node_constant.c
+	@echo 'Building file: $<'
+	@echo 'Invoking: ARM gcc compiler'
+	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	@echo 'Finished building: $<'
+	@echo ' '
+
+ext/graph_blocks/node_gain.o: /local/ucart/MicroCART_17-18/quad/src/graph_blocks/node_gain.c
+	@echo 'Building file: $<'
+	@echo 'Invoking: ARM gcc compiler'
+	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	@echo 'Finished building: $<'
+	@echo ' '
+
+ext/graph_blocks/node_mixer.o: /local/ucart/MicroCART_17-18/quad/src/graph_blocks/node_mixer.c
+	@echo 'Building file: $<'
+	@echo 'Invoking: ARM gcc compiler'
+	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	@echo 'Finished building: $<'
+	@echo ' '
+
+ext/graph_blocks/node_mult.o: /local/ucart/MicroCART_17-18/quad/src/graph_blocks/node_mult.c
+	@echo 'Building file: $<'
+	@echo 'Invoking: ARM gcc compiler'
+	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	@echo 'Finished building: $<'
+	@echo ' '
+
+ext/graph_blocks/node_pid.o: /local/ucart/MicroCART_17-18/quad/src/graph_blocks/node_pid.c
+	@echo 'Building file: $<'
+	@echo 'Invoking: ARM gcc compiler'
+	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	@echo 'Finished building: $<'
+	@echo ' '
+
+ext/graph_blocks/node_pow.o: /local/ucart/MicroCART_17-18/quad/src/graph_blocks/node_pow.c
+	@echo 'Building file: $<'
+	@echo 'Invoking: ARM gcc compiler'
+	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	@echo 'Finished building: $<'
+	@echo ' '
+
+
diff --git a/quad/xsdk_workspace/modular_quad_pid/Debug/ext/quad_app/subdir.mk b/quad/xsdk_workspace/modular_quad_pid/Debug/ext/quad_app/subdir.mk
index aa98b08cd518adce7126a0d547300de93bc0dae8..c3cc8861eb96ad0f808c5de24c00d27b8d74a657 100644
--- a/quad/xsdk_workspace/modular_quad_pid/Debug/ext/quad_app/subdir.mk
+++ b/quad/xsdk_workspace/modular_quad_pid/Debug/ext/quad_app/subdir.mk
@@ -7,7 +7,6 @@ C_SRCS += \
 /local/ucart/MicroCART_17-18/quad/src/quad_app/PID.c \
 /local/ucart/MicroCART_17-18/quad/src/quad_app/actuator_command_processing.c \
 /local/ucart/MicroCART_17-18/quad/src/quad_app/callbacks.c \
-/local/ucart/MicroCART_17-18/quad/src/quad_app/commands.c \
 /local/ucart/MicroCART_17-18/quad/src/quad_app/communication.c \
 /local/ucart/MicroCART_17-18/quad/src/quad_app/control_algorithm.c \
 /local/ucart/MicroCART_17-18/quad/src/quad_app/controllers.c \
@@ -17,9 +16,6 @@ C_SRCS += \
 /local/ucart/MicroCART_17-18/quad/src/quad_app/log_data.c \
 /local/ucart/MicroCART_17-18/quad/src/quad_app/mio7_led.c \
 /local/ucart/MicroCART_17-18/quad/src/quad_app/new_log_data.c \
-/local/ucart/MicroCART_17-18/quad/src/quad_app/node_bounds.c \
-/local/ucart/MicroCART_17-18/quad/src/quad_app/node_mixer.c \
-/local/ucart/MicroCART_17-18/quad/src/quad_app/node_pid.c \
 /local/ucart/MicroCART_17-18/quad/src/quad_app/packet_processing.c \
 /local/ucart/MicroCART_17-18/quad/src/quad_app/quad_app.c \
 /local/ucart/MicroCART_17-18/quad/src/quad_app/send_actuator_commands.c \
@@ -34,7 +30,6 @@ OBJS += \
 ./ext/quad_app/PID.o \
 ./ext/quad_app/actuator_command_processing.o \
 ./ext/quad_app/callbacks.o \
-./ext/quad_app/commands.o \
 ./ext/quad_app/communication.o \
 ./ext/quad_app/control_algorithm.o \
 ./ext/quad_app/controllers.o \
@@ -44,9 +39,6 @@ OBJS += \
 ./ext/quad_app/log_data.o \
 ./ext/quad_app/mio7_led.o \
 ./ext/quad_app/new_log_data.o \
-./ext/quad_app/node_bounds.o \
-./ext/quad_app/node_mixer.o \
-./ext/quad_app/node_pid.o \
 ./ext/quad_app/packet_processing.o \
 ./ext/quad_app/quad_app.o \
 ./ext/quad_app/send_actuator_commands.o \
@@ -61,7 +53,6 @@ C_DEPS += \
 ./ext/quad_app/PID.d \
 ./ext/quad_app/actuator_command_processing.d \
 ./ext/quad_app/callbacks.d \
-./ext/quad_app/commands.d \
 ./ext/quad_app/communication.d \
 ./ext/quad_app/control_algorithm.d \
 ./ext/quad_app/controllers.d \
@@ -71,9 +62,6 @@ C_DEPS += \
 ./ext/quad_app/log_data.d \
 ./ext/quad_app/mio7_led.d \
 ./ext/quad_app/new_log_data.d \
-./ext/quad_app/node_bounds.d \
-./ext/quad_app/node_mixer.d \
-./ext/quad_app/node_pid.d \
 ./ext/quad_app/packet_processing.d \
 ./ext/quad_app/quad_app.d \
 ./ext/quad_app/send_actuator_commands.d \
@@ -89,175 +77,147 @@ C_DEPS += \
 ext/quad_app/PID.o: /local/ucart/MicroCART_17-18/quad/src/quad_app/PID.c
 	@echo 'Building file: $<'
 	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
 	@echo 'Finished building: $<'
 	@echo ' '
 
 ext/quad_app/actuator_command_processing.o: /local/ucart/MicroCART_17-18/quad/src/quad_app/actuator_command_processing.c
 	@echo 'Building file: $<'
 	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
 	@echo 'Finished building: $<'
 	@echo ' '
 
 ext/quad_app/callbacks.o: /local/ucart/MicroCART_17-18/quad/src/quad_app/callbacks.c
 	@echo 'Building file: $<'
 	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
-	@echo 'Finished building: $<'
-	@echo ' '
-
-ext/quad_app/commands.o: /local/ucart/MicroCART_17-18/quad/src/quad_app/commands.c
-	@echo 'Building file: $<'
-	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
 	@echo 'Finished building: $<'
 	@echo ' '
 
 ext/quad_app/communication.o: /local/ucart/MicroCART_17-18/quad/src/quad_app/communication.c
 	@echo 'Building file: $<'
 	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
 	@echo 'Finished building: $<'
 	@echo ' '
 
 ext/quad_app/control_algorithm.o: /local/ucart/MicroCART_17-18/quad/src/quad_app/control_algorithm.c
 	@echo 'Building file: $<'
 	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
 	@echo 'Finished building: $<'
 	@echo ' '
 
 ext/quad_app/controllers.o: /local/ucart/MicroCART_17-18/quad/src/quad_app/controllers.c
 	@echo 'Building file: $<'
 	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
 	@echo 'Finished building: $<'
 	@echo ' '
 
 ext/quad_app/conversion.o: /local/ucart/MicroCART_17-18/quad/src/quad_app/conversion.c
 	@echo 'Building file: $<'
 	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
 	@echo 'Finished building: $<'
 	@echo ' '
 
 ext/quad_app/iic_utils.o: /local/ucart/MicroCART_17-18/quad/src/quad_app/iic_utils.c
 	@echo 'Building file: $<'
 	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
 	@echo 'Finished building: $<'
 	@echo ' '
 
 ext/quad_app/initialize_components.o: /local/ucart/MicroCART_17-18/quad/src/quad_app/initialize_components.c
 	@echo 'Building file: $<'
 	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
 	@echo 'Finished building: $<'
 	@echo ' '
 
 ext/quad_app/log_data.o: /local/ucart/MicroCART_17-18/quad/src/quad_app/log_data.c
 	@echo 'Building file: $<'
 	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
 	@echo 'Finished building: $<'
 	@echo ' '
 
 ext/quad_app/mio7_led.o: /local/ucart/MicroCART_17-18/quad/src/quad_app/mio7_led.c
 	@echo 'Building file: $<'
 	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
 	@echo 'Finished building: $<'
 	@echo ' '
 
 ext/quad_app/new_log_data.o: /local/ucart/MicroCART_17-18/quad/src/quad_app/new_log_data.c
 	@echo 'Building file: $<'
 	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
-	@echo 'Finished building: $<'
-	@echo ' '
-
-ext/quad_app/node_bounds.o: /local/ucart/MicroCART_17-18/quad/src/quad_app/node_bounds.c
-	@echo 'Building file: $<'
-	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
-	@echo 'Finished building: $<'
-	@echo ' '
-
-ext/quad_app/node_mixer.o: /local/ucart/MicroCART_17-18/quad/src/quad_app/node_mixer.c
-	@echo 'Building file: $<'
-	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
-	@echo 'Finished building: $<'
-	@echo ' '
-
-ext/quad_app/node_pid.o: /local/ucart/MicroCART_17-18/quad/src/quad_app/node_pid.c
-	@echo 'Building file: $<'
-	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
 	@echo 'Finished building: $<'
 	@echo ' '
 
 ext/quad_app/packet_processing.o: /local/ucart/MicroCART_17-18/quad/src/quad_app/packet_processing.c
 	@echo 'Building file: $<'
 	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
 	@echo 'Finished building: $<'
 	@echo ' '
 
 ext/quad_app/quad_app.o: /local/ucart/MicroCART_17-18/quad/src/quad_app/quad_app.c
 	@echo 'Building file: $<'
 	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
 	@echo 'Finished building: $<'
 	@echo ' '
 
 ext/quad_app/send_actuator_commands.o: /local/ucart/MicroCART_17-18/quad/src/quad_app/send_actuator_commands.c
 	@echo 'Building file: $<'
 	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
 	@echo 'Finished building: $<'
 	@echo ' '
 
 ext/quad_app/sensor.o: /local/ucart/MicroCART_17-18/quad/src/quad_app/sensor.c
 	@echo 'Building file: $<'
 	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
 	@echo 'Finished building: $<'
 	@echo ' '
 
 ext/quad_app/sensor_processing.o: /local/ucart/MicroCART_17-18/quad/src/quad_app/sensor_processing.c
 	@echo 'Building file: $<'
 	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
 	@echo 'Finished building: $<'
 	@echo ' '
 
 ext/quad_app/timer.o: /local/ucart/MicroCART_17-18/quad/src/quad_app/timer.c
 	@echo 'Building file: $<'
 	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
 	@echo 'Finished building: $<'
 	@echo ' '
 
 ext/quad_app/update_gui.o: /local/ucart/MicroCART_17-18/quad/src/quad_app/update_gui.c
 	@echo 'Building file: $<'
 	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
 	@echo 'Finished building: $<'
 	@echo ' '
 
 ext/quad_app/user_input.o: /local/ucart/MicroCART_17-18/quad/src/quad_app/user_input.c
 	@echo 'Building file: $<'
 	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
 	@echo 'Finished building: $<'
 	@echo ' '
 
 ext/quad_app/util.o: /local/ucart/MicroCART_17-18/quad/src/quad_app/util.c
 	@echo 'Building file: $<'
 	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
 	@echo 'Finished building: $<'
 	@echo ' '
 
diff --git a/quad/xsdk_workspace/modular_quad_pid/Debug/ext/queue/subdir.mk b/quad/xsdk_workspace/modular_quad_pid/Debug/ext/queue/subdir.mk
index 62a2b5e38e4227b13d52b1f71a3d59ca2c1154a7..8141db2ad3d477fa611ab8cb815bf102a059e2e3 100644
--- a/quad/xsdk_workspace/modular_quad_pid/Debug/ext/queue/subdir.mk
+++ b/quad/xsdk_workspace/modular_quad_pid/Debug/ext/queue/subdir.mk
@@ -17,7 +17,7 @@ C_DEPS += \
 ext/queue/queue.o: /local/ucart/MicroCART_17-18/quad/src/queue/queue.c
 	@echo 'Building file: $<'
 	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
 	@echo 'Finished building: $<'
 	@echo ' '
 
diff --git a/quad/xsdk_workspace/modular_quad_pid/Debug/makefile b/quad/xsdk_workspace/modular_quad_pid/Debug/makefile
index 1541d5523584e2da44d62a4297e85fb6d6182adf..928243ba307f1025a6d169fd83c3abad121074c9 100644
--- a/quad/xsdk_workspace/modular_quad_pid/Debug/makefile
+++ b/quad/xsdk_workspace/modular_quad_pid/Debug/makefile
@@ -11,7 +11,9 @@ RM := rm -rf
 -include src/subdir.mk
 -include ext/queue/subdir.mk
 -include ext/quad_app/subdir.mk
+-include ext/graph_blocks/subdir.mk
 -include ext/computation_graph/subdir.mk
+-include ext/commands/subdir.mk
 -include subdir.mk
 -include objects.mk
 
diff --git a/quad/xsdk_workspace/modular_quad_pid/Debug/sources.mk b/quad/xsdk_workspace/modular_quad_pid/Debug/sources.mk
index 2ab3ec257c3d1145b8c6781b871c0e3a9e1f6f8d..9b76509427183193e779ddad577b92f06904d6a9 100644
--- a/quad/xsdk_workspace/modular_quad_pid/Debug/sources.mk
+++ b/quad/xsdk_workspace/modular_quad_pid/Debug/sources.mk
@@ -19,5 +19,7 @@ SUBDIRS := \
 src \
 ext/queue \
 ext/quad_app \
+ext/graph_blocks \
 ext/computation_graph \
+ext/commands \
 
diff --git a/quad/xsdk_workspace/modular_quad_pid/Debug/src/subdir.mk b/quad/xsdk_workspace/modular_quad_pid/Debug/src/subdir.mk
index ad5162ebfedbaea78a20e0ab74d85dd782ccb6dc..342be9a4df75c540826513b7781d20f55cec7e6e 100644
--- a/quad/xsdk_workspace/modular_quad_pid/Debug/src/subdir.mk
+++ b/quad/xsdk_workspace/modular_quad_pid/Debug/src/subdir.mk
@@ -53,7 +53,7 @@ C_DEPS += \
 src/%.o: ../src/%.c
 	@echo 'Building file: $<'
 	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	arm-xilinx-eabi-gcc -Wall -O0 -g3 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
 	@echo 'Finished building: $<'
 	@echo ' '
 
diff --git a/quad/xsdk_workspace/modular_quad_pid/Release/ext/commands/subdir.mk b/quad/xsdk_workspace/modular_quad_pid/Release/ext/commands/subdir.mk
new file mode 100644
index 0000000000000000000000000000000000000000..8cdbd185e5ccf1c0be99a0d8fa96060a34e2b513
--- /dev/null
+++ b/quad/xsdk_workspace/modular_quad_pid/Release/ext/commands/subdir.mk
@@ -0,0 +1,24 @@
+################################################################################
+# Automatically-generated file. Do not edit!
+################################################################################
+
+# Add inputs and outputs from these tool invocations to the build variables 
+C_SRCS += \
+/local/ucart/MicroCART_17-18/quad/src/commands/commands.c 
+
+OBJS += \
+./ext/commands/commands.o 
+
+C_DEPS += \
+./ext/commands/commands.d 
+
+
+# Each subdirectory must supply rules for building sources it contributes
+ext/commands/commands.o: /local/ucart/MicroCART_17-18/quad/src/commands/commands.c
+	@echo 'Building file: $<'
+	@echo 'Invoking: ARM gcc compiler'
+	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	@echo 'Finished building: $<'
+	@echo ' '
+
+
diff --git a/quad/xsdk_workspace/modular_quad_pid/Release/ext/computation_graph/subdir.mk b/quad/xsdk_workspace/modular_quad_pid/Release/ext/computation_graph/subdir.mk
index ded259e8facfb900511409b4c0a7bbc52cb3bc30..b7232693ee94ab6bc1248c568cabec7ff1c42604 100644
--- a/quad/xsdk_workspace/modular_quad_pid/Release/ext/computation_graph/subdir.mk
+++ b/quad/xsdk_workspace/modular_quad_pid/Release/ext/computation_graph/subdir.mk
@@ -4,80 +4,20 @@
 
 # Add inputs and outputs from these tool invocations to the build variables 
 C_SRCS += \
-/local/ucart/MicroCART_17-18/quad/src/computation_graph/computation_graph.c \
-/local/ucart/MicroCART_17-18/quad/src/computation_graph/node_accumulator.c \
-/local/ucart/MicroCART_17-18/quad/src/computation_graph/node_add.c \
-/local/ucart/MicroCART_17-18/quad/src/computation_graph/node_constant.c \
-/local/ucart/MicroCART_17-18/quad/src/computation_graph/node_gain.c \
-/local/ucart/MicroCART_17-18/quad/src/computation_graph/node_mult.c \
-/local/ucart/MicroCART_17-18/quad/src/computation_graph/node_pow.c 
+/local/ucart/MicroCART_17-18/quad/src/computation_graph/computation_graph.c 
 
 OBJS += \
-./ext/computation_graph/computation_graph.o \
-./ext/computation_graph/node_accumulator.o \
-./ext/computation_graph/node_add.o \
-./ext/computation_graph/node_constant.o \
-./ext/computation_graph/node_gain.o \
-./ext/computation_graph/node_mult.o \
-./ext/computation_graph/node_pow.o 
+./ext/computation_graph/computation_graph.o 
 
 C_DEPS += \
-./ext/computation_graph/computation_graph.d \
-./ext/computation_graph/node_accumulator.d \
-./ext/computation_graph/node_add.d \
-./ext/computation_graph/node_constant.d \
-./ext/computation_graph/node_gain.d \
-./ext/computation_graph/node_mult.d \
-./ext/computation_graph/node_pow.d 
+./ext/computation_graph/computation_graph.d 
 
 
 # Each subdirectory must supply rules for building sources it contributes
 ext/computation_graph/computation_graph.o: /local/ucart/MicroCART_17-18/quad/src/computation_graph/computation_graph.c
 	@echo 'Building file: $<'
 	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
-	@echo 'Finished building: $<'
-	@echo ' '
-
-ext/computation_graph/node_accumulator.o: /local/ucart/MicroCART_17-18/quad/src/computation_graph/node_accumulator.c
-	@echo 'Building file: $<'
-	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
-	@echo 'Finished building: $<'
-	@echo ' '
-
-ext/computation_graph/node_add.o: /local/ucart/MicroCART_17-18/quad/src/computation_graph/node_add.c
-	@echo 'Building file: $<'
-	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
-	@echo 'Finished building: $<'
-	@echo ' '
-
-ext/computation_graph/node_constant.o: /local/ucart/MicroCART_17-18/quad/src/computation_graph/node_constant.c
-	@echo 'Building file: $<'
-	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
-	@echo 'Finished building: $<'
-	@echo ' '
-
-ext/computation_graph/node_gain.o: /local/ucart/MicroCART_17-18/quad/src/computation_graph/node_gain.c
-	@echo 'Building file: $<'
-	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
-	@echo 'Finished building: $<'
-	@echo ' '
-
-ext/computation_graph/node_mult.o: /local/ucart/MicroCART_17-18/quad/src/computation_graph/node_mult.c
-	@echo 'Building file: $<'
-	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
-	@echo 'Finished building: $<'
-	@echo ' '
-
-ext/computation_graph/node_pow.o: /local/ucart/MicroCART_17-18/quad/src/computation_graph/node_pow.c
-	@echo 'Building file: $<'
-	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
 	@echo 'Finished building: $<'
 	@echo ' '
 
diff --git a/quad/xsdk_workspace/modular_quad_pid/Release/ext/graph_blocks/subdir.mk b/quad/xsdk_workspace/modular_quad_pid/Release/ext/graph_blocks/subdir.mk
new file mode 100644
index 0000000000000000000000000000000000000000..eb92d2e47df69bfc075f7f9f5e9eecc8d7a9f365
--- /dev/null
+++ b/quad/xsdk_workspace/modular_quad_pid/Release/ext/graph_blocks/subdir.mk
@@ -0,0 +1,114 @@
+################################################################################
+# Automatically-generated file. Do not edit!
+################################################################################
+
+# Add inputs and outputs from these tool invocations to the build variables 
+C_SRCS += \
+/local/ucart/MicroCART_17-18/quad/src/graph_blocks/graph_blocks.c \
+/local/ucart/MicroCART_17-18/quad/src/graph_blocks/node_accumulator.c \
+/local/ucart/MicroCART_17-18/quad/src/graph_blocks/node_add.c \
+/local/ucart/MicroCART_17-18/quad/src/graph_blocks/node_bounds.c \
+/local/ucart/MicroCART_17-18/quad/src/graph_blocks/node_constant.c \
+/local/ucart/MicroCART_17-18/quad/src/graph_blocks/node_gain.c \
+/local/ucart/MicroCART_17-18/quad/src/graph_blocks/node_mixer.c \
+/local/ucart/MicroCART_17-18/quad/src/graph_blocks/node_mult.c \
+/local/ucart/MicroCART_17-18/quad/src/graph_blocks/node_pid.c \
+/local/ucart/MicroCART_17-18/quad/src/graph_blocks/node_pow.c 
+
+OBJS += \
+./ext/graph_blocks/graph_blocks.o \
+./ext/graph_blocks/node_accumulator.o \
+./ext/graph_blocks/node_add.o \
+./ext/graph_blocks/node_bounds.o \
+./ext/graph_blocks/node_constant.o \
+./ext/graph_blocks/node_gain.o \
+./ext/graph_blocks/node_mixer.o \
+./ext/graph_blocks/node_mult.o \
+./ext/graph_blocks/node_pid.o \
+./ext/graph_blocks/node_pow.o 
+
+C_DEPS += \
+./ext/graph_blocks/graph_blocks.d \
+./ext/graph_blocks/node_accumulator.d \
+./ext/graph_blocks/node_add.d \
+./ext/graph_blocks/node_bounds.d \
+./ext/graph_blocks/node_constant.d \
+./ext/graph_blocks/node_gain.d \
+./ext/graph_blocks/node_mixer.d \
+./ext/graph_blocks/node_mult.d \
+./ext/graph_blocks/node_pid.d \
+./ext/graph_blocks/node_pow.d 
+
+
+# Each subdirectory must supply rules for building sources it contributes
+ext/graph_blocks/graph_blocks.o: /local/ucart/MicroCART_17-18/quad/src/graph_blocks/graph_blocks.c
+	@echo 'Building file: $<'
+	@echo 'Invoking: ARM gcc compiler'
+	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	@echo 'Finished building: $<'
+	@echo ' '
+
+ext/graph_blocks/node_accumulator.o: /local/ucart/MicroCART_17-18/quad/src/graph_blocks/node_accumulator.c
+	@echo 'Building file: $<'
+	@echo 'Invoking: ARM gcc compiler'
+	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	@echo 'Finished building: $<'
+	@echo ' '
+
+ext/graph_blocks/node_add.o: /local/ucart/MicroCART_17-18/quad/src/graph_blocks/node_add.c
+	@echo 'Building file: $<'
+	@echo 'Invoking: ARM gcc compiler'
+	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	@echo 'Finished building: $<'
+	@echo ' '
+
+ext/graph_blocks/node_bounds.o: /local/ucart/MicroCART_17-18/quad/src/graph_blocks/node_bounds.c
+	@echo 'Building file: $<'
+	@echo 'Invoking: ARM gcc compiler'
+	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	@echo 'Finished building: $<'
+	@echo ' '
+
+ext/graph_blocks/node_constant.o: /local/ucart/MicroCART_17-18/quad/src/graph_blocks/node_constant.c
+	@echo 'Building file: $<'
+	@echo 'Invoking: ARM gcc compiler'
+	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	@echo 'Finished building: $<'
+	@echo ' '
+
+ext/graph_blocks/node_gain.o: /local/ucart/MicroCART_17-18/quad/src/graph_blocks/node_gain.c
+	@echo 'Building file: $<'
+	@echo 'Invoking: ARM gcc compiler'
+	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	@echo 'Finished building: $<'
+	@echo ' '
+
+ext/graph_blocks/node_mixer.o: /local/ucart/MicroCART_17-18/quad/src/graph_blocks/node_mixer.c
+	@echo 'Building file: $<'
+	@echo 'Invoking: ARM gcc compiler'
+	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	@echo 'Finished building: $<'
+	@echo ' '
+
+ext/graph_blocks/node_mult.o: /local/ucart/MicroCART_17-18/quad/src/graph_blocks/node_mult.c
+	@echo 'Building file: $<'
+	@echo 'Invoking: ARM gcc compiler'
+	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	@echo 'Finished building: $<'
+	@echo ' '
+
+ext/graph_blocks/node_pid.o: /local/ucart/MicroCART_17-18/quad/src/graph_blocks/node_pid.c
+	@echo 'Building file: $<'
+	@echo 'Invoking: ARM gcc compiler'
+	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	@echo 'Finished building: $<'
+	@echo ' '
+
+ext/graph_blocks/node_pow.o: /local/ucart/MicroCART_17-18/quad/src/graph_blocks/node_pow.c
+	@echo 'Building file: $<'
+	@echo 'Invoking: ARM gcc compiler'
+	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	@echo 'Finished building: $<'
+	@echo ' '
+
+
diff --git a/quad/xsdk_workspace/modular_quad_pid/Release/ext/quad_app/subdir.mk b/quad/xsdk_workspace/modular_quad_pid/Release/ext/quad_app/subdir.mk
index e98b994aea2b9cb3a2ba124842a4cf5c74226d9e..d549e80dffda190f971af055c1a0540a81b5d411 100644
--- a/quad/xsdk_workspace/modular_quad_pid/Release/ext/quad_app/subdir.mk
+++ b/quad/xsdk_workspace/modular_quad_pid/Release/ext/quad_app/subdir.mk
@@ -7,7 +7,6 @@ C_SRCS += \
 /local/ucart/MicroCART_17-18/quad/src/quad_app/PID.c \
 /local/ucart/MicroCART_17-18/quad/src/quad_app/actuator_command_processing.c \
 /local/ucart/MicroCART_17-18/quad/src/quad_app/callbacks.c \
-/local/ucart/MicroCART_17-18/quad/src/quad_app/commands.c \
 /local/ucart/MicroCART_17-18/quad/src/quad_app/communication.c \
 /local/ucart/MicroCART_17-18/quad/src/quad_app/control_algorithm.c \
 /local/ucart/MicroCART_17-18/quad/src/quad_app/controllers.c \
@@ -17,9 +16,6 @@ C_SRCS += \
 /local/ucart/MicroCART_17-18/quad/src/quad_app/log_data.c \
 /local/ucart/MicroCART_17-18/quad/src/quad_app/mio7_led.c \
 /local/ucart/MicroCART_17-18/quad/src/quad_app/new_log_data.c \
-/local/ucart/MicroCART_17-18/quad/src/quad_app/node_bounds.c \
-/local/ucart/MicroCART_17-18/quad/src/quad_app/node_mixer.c \
-/local/ucart/MicroCART_17-18/quad/src/quad_app/node_pid.c \
 /local/ucart/MicroCART_17-18/quad/src/quad_app/packet_processing.c \
 /local/ucart/MicroCART_17-18/quad/src/quad_app/quad_app.c \
 /local/ucart/MicroCART_17-18/quad/src/quad_app/send_actuator_commands.c \
@@ -34,7 +30,6 @@ OBJS += \
 ./ext/quad_app/PID.o \
 ./ext/quad_app/actuator_command_processing.o \
 ./ext/quad_app/callbacks.o \
-./ext/quad_app/commands.o \
 ./ext/quad_app/communication.o \
 ./ext/quad_app/control_algorithm.o \
 ./ext/quad_app/controllers.o \
@@ -44,9 +39,6 @@ OBJS += \
 ./ext/quad_app/log_data.o \
 ./ext/quad_app/mio7_led.o \
 ./ext/quad_app/new_log_data.o \
-./ext/quad_app/node_bounds.o \
-./ext/quad_app/node_mixer.o \
-./ext/quad_app/node_pid.o \
 ./ext/quad_app/packet_processing.o \
 ./ext/quad_app/quad_app.o \
 ./ext/quad_app/send_actuator_commands.o \
@@ -61,7 +53,6 @@ C_DEPS += \
 ./ext/quad_app/PID.d \
 ./ext/quad_app/actuator_command_processing.d \
 ./ext/quad_app/callbacks.d \
-./ext/quad_app/commands.d \
 ./ext/quad_app/communication.d \
 ./ext/quad_app/control_algorithm.d \
 ./ext/quad_app/controllers.d \
@@ -71,9 +62,6 @@ C_DEPS += \
 ./ext/quad_app/log_data.d \
 ./ext/quad_app/mio7_led.d \
 ./ext/quad_app/new_log_data.d \
-./ext/quad_app/node_bounds.d \
-./ext/quad_app/node_mixer.d \
-./ext/quad_app/node_pid.d \
 ./ext/quad_app/packet_processing.d \
 ./ext/quad_app/quad_app.d \
 ./ext/quad_app/send_actuator_commands.d \
@@ -89,175 +77,147 @@ C_DEPS += \
 ext/quad_app/PID.o: /local/ucart/MicroCART_17-18/quad/src/quad_app/PID.c
 	@echo 'Building file: $<'
 	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
 	@echo 'Finished building: $<'
 	@echo ' '
 
 ext/quad_app/actuator_command_processing.o: /local/ucart/MicroCART_17-18/quad/src/quad_app/actuator_command_processing.c
 	@echo 'Building file: $<'
 	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
 	@echo 'Finished building: $<'
 	@echo ' '
 
 ext/quad_app/callbacks.o: /local/ucart/MicroCART_17-18/quad/src/quad_app/callbacks.c
 	@echo 'Building file: $<'
 	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
-	@echo 'Finished building: $<'
-	@echo ' '
-
-ext/quad_app/commands.o: /local/ucart/MicroCART_17-18/quad/src/quad_app/commands.c
-	@echo 'Building file: $<'
-	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
 	@echo 'Finished building: $<'
 	@echo ' '
 
 ext/quad_app/communication.o: /local/ucart/MicroCART_17-18/quad/src/quad_app/communication.c
 	@echo 'Building file: $<'
 	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
 	@echo 'Finished building: $<'
 	@echo ' '
 
 ext/quad_app/control_algorithm.o: /local/ucart/MicroCART_17-18/quad/src/quad_app/control_algorithm.c
 	@echo 'Building file: $<'
 	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
 	@echo 'Finished building: $<'
 	@echo ' '
 
 ext/quad_app/controllers.o: /local/ucart/MicroCART_17-18/quad/src/quad_app/controllers.c
 	@echo 'Building file: $<'
 	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
 	@echo 'Finished building: $<'
 	@echo ' '
 
 ext/quad_app/conversion.o: /local/ucart/MicroCART_17-18/quad/src/quad_app/conversion.c
 	@echo 'Building file: $<'
 	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
 	@echo 'Finished building: $<'
 	@echo ' '
 
 ext/quad_app/iic_utils.o: /local/ucart/MicroCART_17-18/quad/src/quad_app/iic_utils.c
 	@echo 'Building file: $<'
 	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
 	@echo 'Finished building: $<'
 	@echo ' '
 
 ext/quad_app/initialize_components.o: /local/ucart/MicroCART_17-18/quad/src/quad_app/initialize_components.c
 	@echo 'Building file: $<'
 	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
 	@echo 'Finished building: $<'
 	@echo ' '
 
 ext/quad_app/log_data.o: /local/ucart/MicroCART_17-18/quad/src/quad_app/log_data.c
 	@echo 'Building file: $<'
 	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
 	@echo 'Finished building: $<'
 	@echo ' '
 
 ext/quad_app/mio7_led.o: /local/ucart/MicroCART_17-18/quad/src/quad_app/mio7_led.c
 	@echo 'Building file: $<'
 	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
 	@echo 'Finished building: $<'
 	@echo ' '
 
 ext/quad_app/new_log_data.o: /local/ucart/MicroCART_17-18/quad/src/quad_app/new_log_data.c
 	@echo 'Building file: $<'
 	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
-	@echo 'Finished building: $<'
-	@echo ' '
-
-ext/quad_app/node_bounds.o: /local/ucart/MicroCART_17-18/quad/src/quad_app/node_bounds.c
-	@echo 'Building file: $<'
-	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
-	@echo 'Finished building: $<'
-	@echo ' '
-
-ext/quad_app/node_mixer.o: /local/ucart/MicroCART_17-18/quad/src/quad_app/node_mixer.c
-	@echo 'Building file: $<'
-	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
-	@echo 'Finished building: $<'
-	@echo ' '
-
-ext/quad_app/node_pid.o: /local/ucart/MicroCART_17-18/quad/src/quad_app/node_pid.c
-	@echo 'Building file: $<'
-	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
 	@echo 'Finished building: $<'
 	@echo ' '
 
 ext/quad_app/packet_processing.o: /local/ucart/MicroCART_17-18/quad/src/quad_app/packet_processing.c
 	@echo 'Building file: $<'
 	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
 	@echo 'Finished building: $<'
 	@echo ' '
 
 ext/quad_app/quad_app.o: /local/ucart/MicroCART_17-18/quad/src/quad_app/quad_app.c
 	@echo 'Building file: $<'
 	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
 	@echo 'Finished building: $<'
 	@echo ' '
 
 ext/quad_app/send_actuator_commands.o: /local/ucart/MicroCART_17-18/quad/src/quad_app/send_actuator_commands.c
 	@echo 'Building file: $<'
 	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
 	@echo 'Finished building: $<'
 	@echo ' '
 
 ext/quad_app/sensor.o: /local/ucart/MicroCART_17-18/quad/src/quad_app/sensor.c
 	@echo 'Building file: $<'
 	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
 	@echo 'Finished building: $<'
 	@echo ' '
 
 ext/quad_app/sensor_processing.o: /local/ucart/MicroCART_17-18/quad/src/quad_app/sensor_processing.c
 	@echo 'Building file: $<'
 	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
 	@echo 'Finished building: $<'
 	@echo ' '
 
 ext/quad_app/timer.o: /local/ucart/MicroCART_17-18/quad/src/quad_app/timer.c
 	@echo 'Building file: $<'
 	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
 	@echo 'Finished building: $<'
 	@echo ' '
 
 ext/quad_app/update_gui.o: /local/ucart/MicroCART_17-18/quad/src/quad_app/update_gui.c
 	@echo 'Building file: $<'
 	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
 	@echo 'Finished building: $<'
 	@echo ' '
 
 ext/quad_app/user_input.o: /local/ucart/MicroCART_17-18/quad/src/quad_app/user_input.c
 	@echo 'Building file: $<'
 	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
 	@echo 'Finished building: $<'
 	@echo ' '
 
 ext/quad_app/util.o: /local/ucart/MicroCART_17-18/quad/src/quad_app/util.c
 	@echo 'Building file: $<'
 	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
 	@echo 'Finished building: $<'
 	@echo ' '
 
diff --git a/quad/xsdk_workspace/modular_quad_pid/Release/ext/queue/subdir.mk b/quad/xsdk_workspace/modular_quad_pid/Release/ext/queue/subdir.mk
index f1f9d0801d552b2bcdec436544b890e0720c5f30..a595cf53e1bd98da9251d9166acf07f43e6f6651 100644
--- a/quad/xsdk_workspace/modular_quad_pid/Release/ext/queue/subdir.mk
+++ b/quad/xsdk_workspace/modular_quad_pid/Release/ext/queue/subdir.mk
@@ -17,7 +17,7 @@ C_DEPS += \
 ext/queue/queue.o: /local/ucart/MicroCART_17-18/quad/src/queue/queue.c
 	@echo 'Building file: $<'
 	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
 	@echo 'Finished building: $<'
 	@echo ' '
 
diff --git a/quad/xsdk_workspace/modular_quad_pid/Release/makefile b/quad/xsdk_workspace/modular_quad_pid/Release/makefile
index 1541d5523584e2da44d62a4297e85fb6d6182adf..928243ba307f1025a6d169fd83c3abad121074c9 100644
--- a/quad/xsdk_workspace/modular_quad_pid/Release/makefile
+++ b/quad/xsdk_workspace/modular_quad_pid/Release/makefile
@@ -11,7 +11,9 @@ RM := rm -rf
 -include src/subdir.mk
 -include ext/queue/subdir.mk
 -include ext/quad_app/subdir.mk
+-include ext/graph_blocks/subdir.mk
 -include ext/computation_graph/subdir.mk
+-include ext/commands/subdir.mk
 -include subdir.mk
 -include objects.mk
 
diff --git a/quad/xsdk_workspace/modular_quad_pid/Release/sources.mk b/quad/xsdk_workspace/modular_quad_pid/Release/sources.mk
index 2ab3ec257c3d1145b8c6781b871c0e3a9e1f6f8d..9b76509427183193e779ddad577b92f06904d6a9 100644
--- a/quad/xsdk_workspace/modular_quad_pid/Release/sources.mk
+++ b/quad/xsdk_workspace/modular_quad_pid/Release/sources.mk
@@ -19,5 +19,7 @@ SUBDIRS := \
 src \
 ext/queue \
 ext/quad_app \
+ext/graph_blocks \
 ext/computation_graph \
+ext/commands \
 
diff --git a/quad/xsdk_workspace/modular_quad_pid/Release/src/subdir.mk b/quad/xsdk_workspace/modular_quad_pid/Release/src/subdir.mk
index 1ea77e8c1f87203439ad7a5f60f37e2828a341cc..d73a8e4edf8f1be1d1cb7bbc2866698e8091f6d0 100644
--- a/quad/xsdk_workspace/modular_quad_pid/Release/src/subdir.mk
+++ b/quad/xsdk_workspace/modular_quad_pid/Release/src/subdir.mk
@@ -53,7 +53,7 @@ C_DEPS += \
 src/%.o: ../src/%.c
 	@echo 'Building file: $<'
 	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -c -fmessage-length=0 -I../../system_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	arm-xilinx-eabi-gcc -DNDEBUG=1 -Wall -O2 -I../../system_bsp/ps7_cortexa9_0/include -I"/local/ucart/MicroCART_17-18/quad/src/computation_graph" -I"/local/ucart/MicroCART_17-18/quad/src/quad_app" -I"/local/ucart/MicroCART_17-18/quad/src/queue" -I"/local/ucart/MicroCART_17-18/quad/src/commands" -I"/local/ucart/MicroCART_17-18/quad/src/graph_blocks" -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
 	@echo 'Finished building: $<'
 	@echo ' '