diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json
index 0bcbf3d..c4fcf89 100644
--- a/.vscode/c_cpp_properties.json
+++ b/.vscode/c_cpp_properties.json
@@ -19,6 +19,16 @@
 			"cppStandard": "c++17",
 			"intelliSenseMode": "linux-gcc-arm",
 			"configurationProvider": "ms-vscode.cmake-tools"
+		},
+		{
+			"name": "Pico (Ubuntu)",
+			"includePath": ["${workspaceFolder}/**", "${env:PICO_SDK_PATH}/**"],
+			"defines": [],
+			"compilerPath": "/usr/bin/arm-none-eabi-gcc",
+			"cStandard": "c11",
+			"cppStandard": "c++17",
+			"intelliSenseMode": "linux-gcc-arm",
+			"configurationProvider": "ms-vscode.cmake-tools"
 		}
 	],
 	"version": 4
diff --git a/.vscode/settings.json b/.vscode/settings.json
index 771ec68..adf5de5 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -22,5 +22,12 @@
     "CMAKE_MODULE_PATH": "${env:PICO_INSTALL_PATH}/pico-sdk-tools"
   },
   "C_Cpp.default.configurationProvider": "ms-vscode.cmake-tools",
-  "C_Cpp.autoAddFileAssociations": false
+  "C_Cpp.autoAddFileAssociations": false,
+  "C_Cpp.vcFormat.newLine.beforeOpenBrace.type": "newLine",
+  "C_Cpp.formatting": "vcFormat",
+  "C_Cpp.vcFormat.newLine.beforeOpenBrace.block": "newLine",
+  "C_Cpp.vcFormat.newLine.beforeOpenBrace.function": "newLine",
+  "C_Cpp.vcFormat.newLine.beforeOpenBrace.lambda": "newLine",
+  "C_Cpp.vcFormat.newLine.beforeOpenBrace.namespace": "newLine",
+  "C_Cpp.vcFormat.indent.accessSpecifiers": false
 }
diff --git a/CMakeLists.txt b/CMakeLists.txt
index edf1f6c..bae5833 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -4,6 +4,10 @@ cmake_minimum_required(VERSION 3.13)
 # Include build functions from Pico SDK
 include($ENV{PICO_SDK_PATH}/external/pico_sdk_import.cmake)
 
+include_directories(include/)
+file(GLOB SOURCE "src/*.cpp")
+file(GLOB HEADER "include/*.hpp")
+
 # Set name of project (as PROJECT_NAME) and C/C   standards
 project(inverter-control C CXX ASM)
 set(CMAKE_C_STANDARD 11)
@@ -14,6 +18,8 @@ pico_sdk_init()
 
 # Tell CMake where to find the executable source file
 add_executable(${PROJECT_NAME} 
+    ${HEADER}
+    ${SOURCE}
     src/main.cpp
 )
 
diff --git a/include/pod_communication.hpp b/include/pod_communication.hpp
new file mode 100644
index 0000000..5da773b
--- /dev/null
+++ b/include/pod_communication.hpp
@@ -0,0 +1,12 @@
+#ifndef POD_COMMUNICATION_HPP
+#define POD_COMMUNICATION_HPP
+
+struct LimControlMessage
+{
+	float velocity;
+	float throttle;
+};
+
+LimControlMessage read_control_message();
+
+#endif
diff --git a/src/main.cpp b/src/main.cpp
index 5b81872..a9c3dd9 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -1,8 +1,11 @@
+#include <cmath>
+#include <vector>
+
 #include "pico/stdlib.h"
 #include "hardware/gpio.h"
+#include "tusb.h"
 
-#include <cmath>
-#include <vector>
+#include "pod_communication.hpp"
 
 const unsigned PIN_LOGIC = 28;
 const unsigned PIN_ENABLE = 14;
@@ -17,9 +20,10 @@ constexpr bool TEST_CIRCUIT = false;
 
 void initialize_pins()
 {
-	const std::vector<unsigned> pins = TEST_CIRCUIT ? 
+	const std::vector<unsigned> pins = TEST_CIRCUIT ?
 		std::vector<unsigned>{PIN_LOGIC, PIN_ENABLE} :
-		std::vector<unsigned>{pin_H, pin_L};
+		std::vector<unsigned>{ pin_H, pin_L };
+
 	for (unsigned pin : pins)
 	{
 		gpio_init(pin);
@@ -35,7 +39,6 @@ float calculate_frequency(float velocity, float throttle)
 	float g = 0.0305;        // air gap between stators (meters)
 	float mu_r = 1.00000037; // relative permeability of air
 
-
 	// The thrust equation has the form F = Bs / (C + As^2)
 	// which has a peak value at s = √(C/A)
 	// where C is the square of the magnetic sensitivity
@@ -56,7 +59,8 @@ float calculate_frequency(float velocity, float throttle)
 	return (slip + velocity) * 2 * M_PI / L;
 }
 
-void set_logic_pin_(bool v) {
+void set_logic_pin_(bool v)
+{
 	gpio_put(PIN_LOGIC, v);
 	gpio_put(PIN_ENABLE, 1);
 }
@@ -66,10 +70,13 @@ void set_hilo_pins_(bool v)
 	// Even though we should not need to manually introduce a delay (deadtime),
 	// we should still ensure that the rise always occurs after the fall on the
 	// GPIO pins for each phase.
-	if (v) {
+	if (v)
+	{
 		gpio_put(pin_L, !v);
 		gpio_put(pin_H, v);
-	} else {
+	}
+	else
+	{
 		gpio_put(pin_H, v);
 		gpio_put(pin_L, !v);
 	}
@@ -91,21 +98,25 @@ void run_inverter_cycle(int N, float amplitude)
 	}
 }
 
-int frequency_to_samples(float frequency) {
+int frequency_to_samples(float frequency)
+{
 	return OPERATING_FREQUENCY / frequency - OFFSET;
 }
 
-float get_frequency() {
-	return 1;
-}	
-
 int main()
 {
+	stdio_init_all();
+
+	// Wait until USB device is connected
+	while (!tud_cdc_connected())
+		sleep_ms(250);
+
 	initialize_pins();
 
 	while (true)
 	{
-		float frequency = get_frequency();
+		LimControlMessage message = read_control_message();
+		float frequency = calculate_frequency(message.velocity, message.throttle);
 		int N = frequency_to_samples(frequency);
 		run_inverter_cycle(N, 1);
 	}
diff --git a/src/pod_communication.cpp b/src/pod_communication.cpp
new file mode 100644
index 0000000..fbf1076
--- /dev/null
+++ b/src/pod_communication.cpp
@@ -0,0 +1,14 @@
+#include <iostream>
+#include "pod_communication.hpp"
+
+LimControlMessage read_control_message()
+{
+	float velocity;
+	float throttle;
+	std::cin >> velocity >> throttle;
+
+	std::cout << "Received velocity: " << velocity << std::endl;
+	std::cout << "Received throttle: " << throttle << std::endl;
+
+	return { velocity, throttle };
+}